<commit_before>// Copyright (c) 2012 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "content/browser/accessibility/browser_accessibility_manager.h"

#include "base/logging.h"
#include "content/browser/accessibility/browser_accessibility.h"
#include "content/common/accessibility_messages.h"

using webkit_glue::WebAccessibility;

BrowserAccessibility* BrowserAccessibilityFactory::Create() {
  return BrowserAccessibility::Create();
}

// Start child IDs at -1 and decrement each time, because clients use
// child IDs of 1, 2, 3, ... to access the children of an object by
// index, so we use negative IDs to clearly distinguish between indices
// and unique IDs.
// static
int32 BrowserAccessibilityManager::next_child_id_ = -1;

#if (defined(OS_POSIX) && !defined(OS_MACOSX)) || defined(USE_AURA)
// There's no OS-specific implementation of BrowserAccessibilityManager
// on Unix, so just instantiate the base class.
// static
BrowserAccessibilityManager* BrowserAccessibilityManager::Create(
    gfx::NativeView parent_view,
    const WebAccessibility& src,
    BrowserAccessibilityDelegate* delegate,
    BrowserAccessibilityFactory* factory) {
  return new BrowserAccessibilityManager(
      parent_view, src, delegate, factory);
}
#endif

// static
BrowserAccessibilityManager* BrowserAccessibilityManager::CreateEmptyDocument(
    gfx::NativeView parent_view,
    WebAccessibility::State state,
    BrowserAccessibilityDelegate* delegate,
    BrowserAccessibilityFactory* factory) {
  // Use empty document to process notifications
  webkit_glue::WebAccessibility empty_document;
  empty_document.id = 0;
  empty_document.role = WebAccessibility::ROLE_ROOT_WEB_AREA;
  empty_document.state = state;
  return BrowserAccessibilityManager::Create(
      parent_view, empty_document, delegate, factory);
}

BrowserAccessibilityManager::BrowserAccessibilityManager(
    gfx::NativeView parent_view,
    const WebAccessibility& src,
    BrowserAccessibilityDelegate* delegate,
    BrowserAccessibilityFactory* factory)
    : parent_view_(parent_view),
      delegate_(delegate),
      factory_(factory),
      focus_(NULL) {
  root_ = CreateAccessibilityTree(NULL, src, 0, false);
  if (!focus_)
    SetFocus(root_, false);
}

// static
int32 BrowserAccessibilityManager::GetNextChildID() {
  // Get the next child ID, and wrap around when we get near the end
  // of a 32-bit integer range. It's okay to wrap around; we just want
  // to avoid it as long as possible because clients may cache the ID of
  // an object for a while to determine if they've seen it before.
  next_child_id_--;
  if (next_child_id_ == -2000000000)
    next_child_id_ = -1;

  return next_child_id_;
}

BrowserAccessibilityManager::~BrowserAccessibilityManager() {
  // Clients could still hold references to some nodes of the tree, so
  // calling InternalReleaseReference will make sure that as many nodes
  // as possible are released now, and remaining nodes are marked as
  // inactive so that calls to any methods on them will fail gracefully.
  focus_->InternalReleaseReference(false);
  root_->InternalReleaseReference(true);
}

BrowserAccessibility* BrowserAccessibilityManager::GetRoot() {
  return root_;
}

BrowserAccessibility* BrowserAccessibilityManager::GetFromChildID(
    int32 child_id) {
  base::hash_map<int32, BrowserAccessibility*>::iterator iter =
      child_id_map_.find(child_id);
  if (iter != child_id_map_.end()) {
    return iter->second;
  } else {
    return NULL;
  }
}

BrowserAccessibility* BrowserAccessibilityManager::GetFromRendererID(
    int32 renderer_id) {
  base::hash_map<int32, int32>::iterator iter =
      renderer_id_to_child_id_map_.find(renderer_id);
  if (iter == renderer_id_to_child_id_map_.end())
    return NULL;

  int32 child_id = iter->second;
  return GetFromChildID(child_id);
}

void BrowserAccessibilityManager::GotFocus() {
  if (!focus_)
    return;

  NotifyAccessibilityEvent(AccessibilityNotificationFocusChanged, focus_);
}

void BrowserAccessibilityManager::Remove(int32 child_id, int32 renderer_id) {
  child_id_map_.erase(child_id);

  // TODO(ctguil): Investigate if hit. We should never have a newer entry.
  DCHECK(renderer_id_to_child_id_map_[renderer_id] == child_id);
  // Make sure we don't overwrite a newer entry (see UpdateNode for a possible
  // corner case).
  if (renderer_id_to_child_id_map_[renderer_id] == child_id)
    renderer_id_to_child_id_map_.erase(renderer_id);
}

void BrowserAccessibilityManager::OnAccessibilityNotifications(
    const std::vector<AccessibilityHostMsg_NotificationParams>& params) {
  for (uint32 index = 0; index < params.size(); index++) {
    const AccessibilityHostMsg_NotificationParams& param = params[index];

    // Update the tree.
    UpdateNode(param.acc_tree, param.includes_children);

    // Find the node corresponding to the id that's the target of the
    // notification (which may not be the root of the update tree).
    base::hash_map<int32, int32>::iterator iter =
        renderer_id_to_child_id_map_.find(param.id);
    if (iter == renderer_id_to_child_id_map_.end()) {
      continue;
    }
    int32 child_id = iter->second;
    BrowserAccessibility* node = GetFromChildID(child_id);
    if (!node) {
      NOTREACHED();
      continue;
    }

    int notification_type = param.notification_type;
    if (notification_type == AccessibilityNotificationFocusChanged) {
      SetFocus(node, false);

      // Don't send a native focus event if the window itself doesn't
      // have focus.
      if (delegate_ && !delegate_->HasFocus())
        continue;
    }

    // Send the notification event to the operating system.
    NotifyAccessibilityEvent(notification_type, node);

    // Set initial focus when a page is loaded.
    if (notification_type == AccessibilityNotificationLoadComplete) {
      if (!focus_)
        SetFocus(root_, false);
      if (!delegate_ || delegate_->HasFocus())
        NotifyAccessibilityEvent(AccessibilityNotificationFocusChanged, focus_);
    }
  }
}

gfx::NativeView BrowserAccessibilityManager::GetParentView() {
  return parent_view_;
}

BrowserAccessibility* BrowserAccessibilityManager::GetFocus(
    BrowserAccessibility* root) {
  if (focus_ && (!root || focus_->IsDescendantOf(root)))
    return focus_;

  return NULL;
}

void BrowserAccessibilityManager::SetFocus(
    BrowserAccessibility* node, bool notify) {
  if (focus_ != node) {
    if (focus_)
      focus_->InternalReleaseReference(false);
    focus_ = node;
    if (focus_)
      focus_->InternalAddReference();
  }

  if (notify && node && delegate_)
    delegate_->SetAccessibilityFocus(node->renderer_id());
}

void BrowserAccessibilityManager::DoDefaultAction(
    const BrowserAccessibility& node) {
  if (delegate_)
    delegate_->AccessibilityDoDefaultAction(node.renderer_id());
}

void BrowserAccessibilityManager::ScrollToMakeVisible(
    const BrowserAccessibility& node, gfx::Rect subfocus) {
  if (delegate_) {
    delegate_->AccessibilityScrollToMakeVisible(node.renderer_id(), subfocus);
  }
}

void BrowserAccessibilityManager::ScrollToPoint(
    const BrowserAccessibility& node, gfx::Point point) {
  if (delegate_) {
    delegate_->AccessibilityScrollToPoint(node.renderer_id(), point);
  }
}

void BrowserAccessibilityManager::SetTextSelection(
    const BrowserAccessibility& node, int start_offset, int end_offset) {
  if (delegate_) {
    delegate_->AccessibilitySetTextSelection(
        node.renderer_id(), start_offset, end_offset);
  }
}

gfx::Rect BrowserAccessibilityManager::GetViewBounds() {
  if (delegate_)
    return delegate_->GetViewBounds();
  return gfx::Rect();
}

void BrowserAccessibilityManager::UpdateNode(
    const WebAccessibility& src,
    bool include_children) {
  BrowserAccessibility* current = NULL;

  // Look for the node to replace. Either we're replacing the whole tree
  // (role is ROOT_WEB_AREA) or we look it up based on its renderer ID.
  if (src.role == WebAccessibility::ROLE_ROOT_WEB_AREA) {
    current = root_;
  } else {
    base::hash_map<int32, int32>::iterator iter =
        renderer_id_to_child_id_map_.find(src.id);
    if (iter != renderer_id_to_child_id_map_.end()) {
      int32 child_id = iter->second;
      current = GetFromChildID(child_id);
    }
  }

  // If we can't find the node to replace, we're out of sync with the
  // renderer (this would be a bug).
  DCHECK(current);
  if (!current)
    return;

  // If this update is just for a single node (|include_children| is false),
  // modify |current| directly and return - no tree changes are needed.
  if (!include_children) {
    DCHECK_EQ(0U, src.children.size());
    current->PreInitialize(
        this,
        current->parent(),
        current->child_id(),
        current->index_in_parent(),
        src);
    current->PostInitialize();
    return;
  }

  BrowserAccessibility* current_parent = current->parent();
  int current_index_in_parent = current->index_in_parent();

  // Detach all of the nodes in the old tree and get a single flat vector
  // of all node pointers.
  std::vector<BrowserAccessibility*> old_tree_nodes;
  current->DetachTree(&old_tree_nodes);

  // Build a new tree, reusing old nodes if possible. Each node that's
  // reused will have its reference count incremented by one.
  current = CreateAccessibilityTree(
      current_parent, src, current_index_in_parent, true);

  // Decrement the reference count of all nodes in the old tree, which will
  // delete any nodes no longer needed.
  for (int i = 0; i < static_cast<int>(old_tree_nodes.size()); i++)
    old_tree_nodes[i]->InternalReleaseReference(false);

  // If the only reference to the focused node is focus_ itself, then the
  // focused node is no longer in the tree, so set the focus to the root.
  if (focus_ && focus_->ref_count() == 1) {
    SetFocus(root_, false);

    if (delegate_ && delegate_->HasFocus())
      NotifyAccessibilityEvent(AccessibilityNotificationBlur, focus_);
  }
}

BrowserAccessibility* BrowserAccessibilityManager::CreateAccessibilityTree(
    BrowserAccessibility* parent,
    const WebAccessibility& src,
    int index_in_parent,
    bool send_show_events) {
  BrowserAccessibility* instance = NULL;
  int32 child_id = 0;
  bool children_can_send_show_events = send_show_events;
  base::hash_map<int32, int32>::iterator iter =
      renderer_id_to_child_id_map_.find(src.id);

  // If a BrowserAccessibility instance for this ID already exists, add a
  // new reference to it and retrieve its children vector.
  if (iter != renderer_id_to_child_id_map_.end()) {
    child_id = iter->second;
    instance = GetFromChildID(child_id);
  }

  // If the node has changed roles, don't reuse a BrowserAccessibility
  // object, that could confuse a screen reader.
  // TODO(dtseng): Investigate when this gets hit; See crbug.com/93095.
  DCHECK(!instance || instance->role() == src.role);

  // If we're reusing a node, it should already be detached from a parent
  // and any children. If not, that means we have a serious bug somewhere,
  // like the same child is reachable from two places in the same tree.
  if (instance && (instance->parent() != NULL || instance->child_count() > 0)) {
    NOTREACHED();
    instance = NULL;
  }

  if (instance) {
    // If we're reusing a node, update its parent and increment its
    // reference count.
    instance->UpdateParent(parent, index_in_parent);
    instance->InternalAddReference();
    send_show_events = false;
  } else {
    // Otherwise, create a new instance.
    instance = factory_->Create();
    child_id = GetNextChildID();
    children_can_send_show_events = false;
  }

  instance->PreInitialize(this, parent, child_id, index_in_parent, src);
  child_id_map_[child_id] = instance;
  renderer_id_to_child_id_map_[src.id] = child_id;

  if ((src.state >> WebAccessibility::STATE_FOCUSED) & 1)
    SetFocus(instance, false);

  for (int i = 0; i < static_cast<int>(src.children.size()); ++i) {
    BrowserAccessibility* child = CreateAccessibilityTree(
        instance, src.children[i], i, children_can_send_show_events);
    instance->AddChild(child);
  }

  if (src.role == WebAccessibility::ROLE_ROOT_WEB_AREA)
    root_ = instance;

  // Note: the purpose of send_show_events and children_can_send_show_events
  // is so that we send a single ObjectShow event for the root of a subtree
  // that just appeared for the first time, but not on any descendant of
  // that subtree.
  if (send_show_events)
    NotifyAccessibilityEvent(AccessibilityNotificationObjectShow, instance);

  instance->PostInitialize();

  return instance;
}
<commit_msg>The browser accessibility manager should transition focus away on receiving the AccessibilityNotificationBlur event.<commit_after>// Copyright (c) 2012 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "content/browser/accessibility/browser_accessibility_manager.h"

#include "base/logging.h"
#include "content/browser/accessibility/browser_accessibility.h"
#include "content/common/accessibility_messages.h"

using webkit_glue::WebAccessibility;

BrowserAccessibility* BrowserAccessibilityFactory::Create() {
  return BrowserAccessibility::Create();
}

// Start child IDs at -1 and decrement each time, because clients use
// child IDs of 1, 2, 3, ... to access the children of an object by
// index, so we use negative IDs to clearly distinguish between indices
// and unique IDs.
// static
int32 BrowserAccessibilityManager::next_child_id_ = -1;

#if (defined(OS_POSIX) && !defined(OS_MACOSX)) || defined(USE_AURA)
// There's no OS-specific implementation of BrowserAccessibilityManager
// on Unix, so just instantiate the base class.
// static
BrowserAccessibilityManager* BrowserAccessibilityManager::Create(
    gfx::NativeView parent_view,
    const WebAccessibility& src,
    BrowserAccessibilityDelegate* delegate,
    BrowserAccessibilityFactory* factory) {
  return new BrowserAccessibilityManager(
      parent_view, src, delegate, factory);
}
#endif

// static
BrowserAccessibilityManager* BrowserAccessibilityManager::CreateEmptyDocument(
    gfx::NativeView parent_view,
    WebAccessibility::State state,
    BrowserAccessibilityDelegate* delegate,
    BrowserAccessibilityFactory* factory) {
  // Use empty document to process notifications
  webkit_glue::WebAccessibility empty_document;
  empty_document.id = 0;
  empty_document.role = WebAccessibility::ROLE_ROOT_WEB_AREA;
  empty_document.state = state;
  return BrowserAccessibilityManager::Create(
      parent_view, empty_document, delegate, factory);
}

BrowserAccessibilityManager::BrowserAccessibilityManager(
    gfx::NativeView parent_view,
    const WebAccessibility& src,
    BrowserAccessibilityDelegate* delegate,
    BrowserAccessibilityFactory* factory)
    : parent_view_(parent_view),
      delegate_(delegate),
      factory_(factory),
      focus_(NULL) {
  root_ = CreateAccessibilityTree(NULL, src, 0, false);
  if (!focus_)
    SetFocus(root_, false);
}

// static
int32 BrowserAccessibilityManager::GetNextChildID() {
  // Get the next child ID, and wrap around when we get near the end
  // of a 32-bit integer range. It's okay to wrap around; we just want
  // to avoid it as long as possible because clients may cache the ID of
  // an object for a while to determine if they've seen it before.
  next_child_id_--;
  if (next_child_id_ == -2000000000)
    next_child_id_ = -1;

  return next_child_id_;
}

BrowserAccessibilityManager::~BrowserAccessibilityManager() {
  // Clients could still hold references to some nodes of the tree, so
  // calling InternalReleaseReference will make sure that as many nodes
  // as possible are released now, and remaining nodes are marked as
  // inactive so that calls to any methods on them will fail gracefully.
  focus_->InternalReleaseReference(false);
  root_->InternalReleaseReference(true);
}

BrowserAccessibility* BrowserAccessibilityManager::GetRoot() {
  return root_;
}

BrowserAccessibility* BrowserAccessibilityManager::GetFromChildID(
    int32 child_id) {
  base::hash_map<int32, BrowserAccessibility*>::iterator iter =
      child_id_map_.find(child_id);
  if (iter != child_id_map_.end()) {
    return iter->second;
  } else {
    return NULL;
  }
}

BrowserAccessibility* BrowserAccessibilityManager::GetFromRendererID(
    int32 renderer_id) {
  base::hash_map<int32, int32>::iterator iter =
      renderer_id_to_child_id_map_.find(renderer_id);
  if (iter == renderer_id_to_child_id_map_.end())
    return NULL;

  int32 child_id = iter->second;
  return GetFromChildID(child_id);
}

void BrowserAccessibilityManager::GotFocus() {
  if (!focus_)
    return;

  NotifyAccessibilityEvent(AccessibilityNotificationFocusChanged, focus_);
}

void BrowserAccessibilityManager::Remove(int32 child_id, int32 renderer_id) {
  child_id_map_.erase(child_id);

  // TODO(ctguil): Investigate if hit. We should never have a newer entry.
  DCHECK(renderer_id_to_child_id_map_[renderer_id] == child_id);
  // Make sure we don't overwrite a newer entry (see UpdateNode for a possible
  // corner case).
  if (renderer_id_to_child_id_map_[renderer_id] == child_id)
    renderer_id_to_child_id_map_.erase(renderer_id);
}

void BrowserAccessibilityManager::OnAccessibilityNotifications(
    const std::vector<AccessibilityHostMsg_NotificationParams>& params) {
  for (uint32 index = 0; index < params.size(); index++) {
    const AccessibilityHostMsg_NotificationParams& param = params[index];

    // Update the tree.
    UpdateNode(param.acc_tree, param.includes_children);

    // Find the node corresponding to the id that's the target of the
    // notification (which may not be the root of the update tree).
    base::hash_map<int32, int32>::iterator iter =
        renderer_id_to_child_id_map_.find(param.id);
    if (iter == renderer_id_to_child_id_map_.end()) {
      continue;
    }
    int32 child_id = iter->second;
    BrowserAccessibility* node = GetFromChildID(child_id);
    if (!node) {
      NOTREACHED();
      continue;
    }

    int notification_type = param.notification_type;
    if (notification_type == AccessibilityNotificationFocusChanged ||
        notification_type == AccessibilityNotificationBlur) {
      SetFocus(node, false);

      // Don't send a native focus event if the window itself doesn't
      // have focus.
      if (delegate_ && !delegate_->HasFocus())
        continue;
    }

    // Send the notification event to the operating system.
    NotifyAccessibilityEvent(notification_type, node);

    // Set initial focus when a page is loaded.
    if (notification_type == AccessibilityNotificationLoadComplete) {
      if (!focus_)
        SetFocus(root_, false);
      if (!delegate_ || delegate_->HasFocus())
        NotifyAccessibilityEvent(AccessibilityNotificationFocusChanged, focus_);
    }
  }
}

gfx::NativeView BrowserAccessibilityManager::GetParentView() {
  return parent_view_;
}

BrowserAccessibility* BrowserAccessibilityManager::GetFocus(
    BrowserAccessibility* root) {
  if (focus_ && (!root || focus_->IsDescendantOf(root)))
    return focus_;

  return NULL;
}

void BrowserAccessibilityManager::SetFocus(
    BrowserAccessibility* node, bool notify) {
  if (focus_ != node) {
    if (focus_)
      focus_->InternalReleaseReference(false);
    focus_ = node;
    if (focus_)
      focus_->InternalAddReference();
  }

  if (notify && node && delegate_)
    delegate_->SetAccessibilityFocus(node->renderer_id());
}

void BrowserAccessibilityManager::DoDefaultAction(
    const BrowserAccessibility& node) {
  if (delegate_)
    delegate_->AccessibilityDoDefaultAction(node.renderer_id());
}

void BrowserAccessibilityManager::ScrollToMakeVisible(
    const BrowserAccessibility& node, gfx::Rect subfocus) {
  if (delegate_) {
    delegate_->AccessibilityScrollToMakeVisible(node.renderer_id(), subfocus);
  }
}

void BrowserAccessibilityManager::ScrollToPoint(
    const BrowserAccessibility& node, gfx::Point point) {
  if (delegate_) {
    delegate_->AccessibilityScrollToPoint(node.renderer_id(), point);
  }
}

void BrowserAccessibilityManager::SetTextSelection(
    const BrowserAccessibility& node, int start_offset, int end_offset) {
  if (delegate_) {
    delegate_->AccessibilitySetTextSelection(
        node.renderer_id(), start_offset, end_offset);
  }
}

gfx::Rect BrowserAccessibilityManager::GetViewBounds() {
  if (delegate_)
    return delegate_->GetViewBounds();
  return gfx::Rect();
}

void BrowserAccessibilityManager::UpdateNode(
    const WebAccessibility& src,
    bool include_children) {
  BrowserAccessibility* current = NULL;

  // Look for the node to replace. Either we're replacing the whole tree
  // (role is ROOT_WEB_AREA) or we look it up based on its renderer ID.
  if (src.role == WebAccessibility::ROLE_ROOT_WEB_AREA) {
    current = root_;
  } else {
    base::hash_map<int32, int32>::iterator iter =
        renderer_id_to_child_id_map_.find(src.id);
    if (iter != renderer_id_to_child_id_map_.end()) {
      int32 child_id = iter->second;
      current = GetFromChildID(child_id);
    }
  }

  // If we can't find the node to replace, we're out of sync with the
  // renderer (this would be a bug).
  DCHECK(current);
  if (!current)
    return;

  // If this update is just for a single node (|include_children| is false),
  // modify |current| directly and return - no tree changes are needed.
  if (!include_children) {
    DCHECK_EQ(0U, src.children.size());
    current->PreInitialize(
        this,
        current->parent(),
        current->child_id(),
        current->index_in_parent(),
        src);
    current->PostInitialize();
    return;
  }

  BrowserAccessibility* current_parent = current->parent();
  int current_index_in_parent = current->index_in_parent();

  // Detach all of the nodes in the old tree and get a single flat vector
  // of all node pointers.
  std::vector<BrowserAccessibility*> old_tree_nodes;
  current->DetachTree(&old_tree_nodes);

  // Build a new tree, reusing old nodes if possible. Each node that's
  // reused will have its reference count incremented by one.
  current = CreateAccessibilityTree(
      current_parent, src, current_index_in_parent, true);

  // Decrement the reference count of all nodes in the old tree, which will
  // delete any nodes no longer needed.
  for (int i = 0; i < static_cast<int>(old_tree_nodes.size()); i++)
    old_tree_nodes[i]->InternalReleaseReference(false);

  // If the only reference to the focused node is focus_ itself, then the
  // focused node is no longer in the tree, so set the focus to the root.
  if (focus_ && focus_->ref_count() == 1) {
    SetFocus(root_, false);

    if (delegate_ && delegate_->HasFocus())
      NotifyAccessibilityEvent(AccessibilityNotificationBlur, focus_);
  }
}

BrowserAccessibility* BrowserAccessibilityManager::CreateAccessibilityTree(
    BrowserAccessibility* parent,
    const WebAccessibility& src,
    int index_in_parent,
    bool send_show_events) {
  BrowserAccessibility* instance = NULL;
  int32 child_id = 0;
  bool children_can_send_show_events = send_show_events;
  base::hash_map<int32, int32>::iterator iter =
      renderer_id_to_child_id_map_.find(src.id);

  // If a BrowserAccessibility instance for this ID already exists, add a
  // new reference to it and retrieve its children vector.
  if (iter != renderer_id_to_child_id_map_.end()) {
    child_id = iter->second;
    instance = GetFromChildID(child_id);
  }

  // If the node has changed roles, don't reuse a BrowserAccessibility
  // object, that could confuse a screen reader.
  // TODO(dtseng): Investigate when this gets hit; See crbug.com/93095.
  DCHECK(!instance || instance->role() == src.role);

  // If we're reusing a node, it should already be detached from a parent
  // and any children. If not, that means we have a serious bug somewhere,
  // like the same child is reachable from two places in the same tree.
  if (instance && (instance->parent() != NULL || instance->child_count() > 0)) {
    NOTREACHED();
    instance = NULL;
  }

  if (instance) {
    // If we're reusing a node, update its parent and increment its
    // reference count.
    instance->UpdateParent(parent, index_in_parent);
    instance->InternalAddReference();
    send_show_events = false;
  } else {
    // Otherwise, create a new instance.
    instance = factory_->Create();
    child_id = GetNextChildID();
    children_can_send_show_events = false;
  }

  instance->PreInitialize(this, parent, child_id, index_in_parent, src);
  child_id_map_[child_id] = instance;
  renderer_id_to_child_id_map_[src.id] = child_id;

  if ((src.state >> WebAccessibility::STATE_FOCUSED) & 1)
    SetFocus(instance, false);

  for (int i = 0; i < static_cast<int>(src.children.size()); ++i) {
    BrowserAccessibility* child = CreateAccessibilityTree(
        instance, src.children[i], i, children_can_send_show_events);
    instance->AddChild(child);
  }

  if (src.role == WebAccessibility::ROLE_ROOT_WEB_AREA)
    root_ = instance;

  // Note: the purpose of send_show_events and children_can_send_show_events
  // is so that we send a single ObjectShow event for the root of a subtree
  // that just appeared for the first time, but not on any descendant of
  // that subtree.
  if (send_show_events)
    NotifyAccessibilityEvent(AccessibilityNotificationObjectShow, instance);

  instance->PostInitialize();

  return instance;
}
<|endoftext|>


<commit_before>#include "viewer/primitives/simple_geometry.hh"
#include "viewer/window_3d.hh"

#include "estimation/jet/jet_filter.hh"
#include "estimation/jet/jet_pose_opt.hh"

#include "estimation/jet/jet_rk4.hh"

#include "util/environment.hh"

#include "eigen.hh"
#include "sophus.hh"

namespace estimation {
namespace jet_filter {
namespace {

Parameters mock_parameters() {
  Parameters p;
  const jcc::Vec3 g(0.0, 0.0, -1.3);
  p.g_world = g;

  // const jcc::Vec3 t(0.1, 0.5, 0.1);
  const jcc::Vec3 t(0.0, 0.1, 0.25);
  // const jcc::Vec3 t = jcc::Vec3::Zero();
  const SO3 r_vehicle_from_sensor = SO3::exp(jcc::Vec3(0.0, 0.0, 0.0));
  const SE3 sensor_from_body(r_vehicle_from_sensor, t);
  p.T_imu_from_vehicle = sensor_from_body;
  return p;
}

void setup() {
  const auto view = viewer::get_window3d("Mr. Filter, filters");
  view->set_target_from_world(
      SE3(SO3::exp(Eigen::Vector3d(-3.1415 * 0.5, 0.0, 0.0)), Eigen::Vector3d::Zero()));
  view->set_continue_time_ms(10);

  const auto background = view->add_primitive<viewer::SimpleGeometry>();
  const geometry::shapes::Plane ground{jcc::Vec3::UnitZ(), 0.0};
  background->add_plane({ground});
  background->flip();
}

void draw_states(viewer::SimpleGeometry& geo,
                 const std::vector<State>& states,
                 bool truth) {
  const int n_states = static_cast<int>(states.size());
  for (int k = 0; k < n_states; ++k) {
    auto& state = states.at(k);
    const SE3 T_world_from_body = state.T_body_from_world.inverse();
    if (truth) {
      geo.add_axes({T_world_from_body, 0.1});
    } else {
      geo.add_axes({T_world_from_body, 0.05, 2.0, true});

      if (k < n_states - 1) {
        const auto& next_state = states.at(k + 1);
        const SE3 T_world_from_body_next = next_state.T_body_from_world.inverse();
        geo.add_line(
            {T_world_from_body.translation(), T_world_from_body_next.translation()});
      }
    }
  }
}

void print_state(const State& x) {
  std::cout << "\teps_dot   : " << x.eps_dot.transpose() << std::endl;
  std::cout << "\teps_ddot  : " << x.eps_ddot.transpose() << std::endl;
  std::cout << "\taccel_bias: " << x.accel_bias.transpose() << std::endl;
  std::cout << "\tgyro_bias : " << x.gyro_bias.transpose() << std::endl;
  // const auto res = observe_accel(xp.x, true_params);
  // std::cout << "\tExpected Measurement: " << res.transpose() << std::endl;
}

template <int dim, int row, int mat_size>
void set_diag_to_value(MatNd<mat_size, mat_size>& mat, double value) {
  mat.template block<dim, dim>(row, row) = (MatNd<dim, dim>::Identity() * value);
}
}  // namespace

class JetOptimizer {
 public:
  JetOptimizer() {
    MatNd<AccelMeasurement::DIM, AccelMeasurement::DIM> accel_cov;
    accel_cov.setZero();
    set_diag_to_value<AccelMeasurementDelta::observed_acceleration_error_dim,
                      AccelMeasurementDelta::observed_acceleration_error_ind>(accel_cov,
                                                                              0.01);

    MatNd<FiducialMeasurement::DIM, FiducialMeasurement::DIM> fiducial_cov;
    fiducial_cov.block<3, 3>(0, 0) = MatNd<3, 3>::Identity() * 0.001;
    fiducial_cov.block<3, 3>(3, 3) = MatNd<3, 3>::Identity() * 0.0001;

    MatNd<State::DIM, State::DIM> state_cov;
    state_cov.setZero();
    set_diag_to_value<StateDelta::accel_bias_error_dim, StateDelta::accel_bias_error_ind>(
        state_cov, 0.0001);
    set_diag_to_value<StateDelta::gyro_bias_error_dim, StateDelta::gyro_bias_error_ind>(
        state_cov, 0.0001);
    set_diag_to_value<StateDelta::eps_dot_error_dim, StateDelta::eps_dot_error_ind>(
        state_cov, 0.1);
    set_diag_to_value<StateDelta::eps_ddot_error_dim, StateDelta::eps_ddot_error_ind>(
        state_cov, 0.1);

    constexpr int T_error_dim = StateDelta::T_body_from_world_error_log_dim;
    constexpr int T_error_ind = StateDelta::T_body_from_world_error_log_ind;
    set_diag_to_value<T_error_dim, T_error_ind>(state_cov, 0.001);

    imu_id_ = pose_opt_.add_error_model<AccelMeasurement>(accel_error_model, accel_cov);
    fiducial_id_ = pose_opt_.add_error_model<FiducialMeasurement>(fiducial_error_model,
                                                                  fiducial_cov);
    pose_opt_.set_dynamics_cov(state_cov);
  }

  void measure_imu(const AccelMeasurement& meas, const TimePoint& t) {
    pose_opt_.add_measurement(meas, t, imu_id_);
  }

  void measure_fiducial(const FiducialMeasurement& meas, const TimePoint& t) {
    pose_opt_.add_measurement(meas, t, fiducial_id_);
  }

  JetPoseOptimizer::Solution solve(const std::vector<State> x, const Parameters& p) {
    const auto view = viewer::get_window3d("Mr. Filter, filters");
    const auto geo = view->add_primitive<viewer::SimpleGeometry>();
    const auto visitor = [&view, &geo](const JetPoseOptimizer::Solution& soln) {
      geo->clear();
      draw_states(*geo, soln.x, false);
      geo->flip();
      std::cout << "\tOptimized g: " << soln.p.g_world.transpose() << std::endl;
      std::cout << "\tOptimized T_imu_from_vehicle: "
                << soln.p.T_imu_from_vehicle.translation().transpose() << "; "
                << soln.p.T_imu_from_vehicle.so3().log().transpose() << std::endl;
      view->spin_until_step();
    };

    return pose_opt_.solve({x, p}, visitor);
  }

 private:
  JetPoseOptimizer pose_opt_{rk4_integrate};
  int imu_id_ = -1;
  int fiducial_id_ = -1;
};

void run_filter() {
  const Parameters true_params = mock_parameters();

  JetOptimizer jet_opt;

  FilterState<State> xp0;
  xp0.P.setIdentity();
  xp0.x.eps_dot[0] = 1.0;
  xp0.x.eps_dot[4] = -0.6;
  xp0.x.eps_dot[5] = -0.2;

  xp0.x.eps_ddot[1] = 0.01;
  xp0.x.eps_ddot[5] = 0.01;

  xp0.time_of_validity = {};

  State true_x = xp0.x;
  true_x.eps_dot[4] = 0.1;

  JetFilter jf(xp0);

  // AccelMeasurement imu_meas;
  // imu_meas.observed_acceleration[0] = 2.0;

  setup();
  const auto view = viewer::get_window3d("Mr. Filter, filters");
  const auto obs_geo = view->add_primitive<viewer::SimpleGeometry>();
  constexpr double sphere_size_m = 0.05;

  std::vector<State> ground_truth;
  std::vector<State> est_states;

  TimePoint start_time = {};
  constexpr auto dt = to_duration(0.1);

  constexpr int NUM_SIM_STEPS = 1000;

  TimePoint current_time = start_time;

  for (int k = 0; k < NUM_SIM_STEPS; ++k) {
    if (k > 75 && k < 130) {
      true_x.eps_ddot[0] = 0.1;
      true_x.eps_ddot[1] = -0.02;
      true_x.eps_ddot[2] = -0.03;

      true_x.eps_ddot[3] = 0.1;
      true_x.eps_ddot[4] = 0.05;
      true_x.eps_ddot[5] = -0.01;
    } else if (k > 130) {
      true_x.eps_ddot.setZero();
    }

    //
    // Accelerometer Observation
    //
    // if ((k % 10 == 0) && k > 75) {
    if (true) {
      ground_truth.push_back(true_x);
      const AccelMeasurement imu_meas =observe_accel(true_x, true_params;
      std::cout << "Accel: " << imu_meas.observed_acceleration.transpose() << std::endl;

      jf.measure_imu(imu_meas, current_time);
      jet_opt.measure_imu(imu_meas, current_time);

      jf.free_run();
      est_states.push_back(jf.state().x);
      assert(jf.state().time_of_validity == current_time);

      const jcc::Vec4 accel_color(0.0, 0.7, 0.7, 0.8);
      obs_geo->add_sphere({jf.state().x.T_body_from_world.inverse().translation(),
                           sphere_size_m, accel_color});

      const SE3 world_from_body = jf.state().x.T_body_from_world.inverse();
      const jcc::Vec3 observed_accel_body_frame =
          (world_from_body.so3() * true_params.T_imu_from_vehicle.so3().inverse() *
           imu_meas.observed_acceleration);

      obs_geo->add_line(
          {world_from_body.translation(),
           world_from_body.translation() + (0.25 * observed_accel_body_frame),
           accel_color});
    }

    //
    // Gyro Observation
    //
    if (false) {
      ground_truth.push_back(true_x);
      const AccelMeasurement imu_meas = observe_accel(true_x, true_params);
      std::cout << "Accel: " << imu_meas.observed_acceleration.transpose() << std::endl;

      jf.measure_imu(imu_meas, current_time);
      jet_opt.measure_imu(imu_meas, current_time);

      jf.free_run();
      est_states.push_back(jf.state().x);
      assert(jf.state().time_of_validity == current_time);

      const jcc::Vec4 accel_color(0.0, 0.7, 0.7, 0.8);
      obs_geo->add_sphere({jf.state().x.T_body_from_world.inverse().translation(),
                           sphere_size_m, accel_color});

      const SE3 world_from_body = jf.state().x.T_body_from_world.inverse();
      const jcc::Vec3 observed_accel_body_frame =
          (world_from_body.so3() * true_params.T_imu_from_vehicle.so3().inverse() *
           imu_meas.observed_acceleration);

      obs_geo->add_line(
          {world_from_body.translation(),
           world_from_body.translation() + (0.25 * observed_accel_body_frame),
           accel_color});
    }

    //
    // Fiducial Measurement
    //

    {
      constexpr auto dt2 = to_duration(0.05);
      true_x = rk4_integrate(true_x, true_params, to_seconds(dt2));
      current_time += dt2;

      ground_truth.push_back(true_x);
      const FiducialMeasurement fiducial_meas = observe_fiducial(true_x, true_params);

      jf.measure_fiducial(fiducial_meas, current_time);
      jet_opt.measure_fiducial(fiducial_meas, current_time);

      jf.free_run();
      est_states.push_back(jf.state().x);
      assert(jf.state().time_of_validity == current_time);
      obs_geo->add_sphere({jf.state().x.T_body_from_world.inverse().translation(),
                           sphere_size_m, jcc::Vec4(1.0, 0.0, 0.0, 1.0)});
    }

    //
    // Printout
    //
    {
      const auto xp = jf.state();
      std::cout << "k: " << k << std::endl;
      print_state(xp.x);
    }
    true_x = rk4_integrate(true_x, true_params, to_seconds(dt));
    current_time += dt;
  }

  // Do the optimization

  obs_geo->flip();
  const auto geo = view->add_primitive<viewer::SimpleGeometry>();
  draw_states(*geo, ground_truth, true);
  geo->flip();

  // const std::vector<State> crap_init(est_states.size(), xp0.x);
  // const auto solution = jet_opt.solve(crap_init, jf.parameters());
  const auto solution = jet_opt.solve(est_states, jf.parameters());
  // const auto solution = jet_opt.solve(ground_truth, jf.parameters());

  for (std::size_t k = 0; k < solution.x.size(); ++k) {
    const auto& state = solution.x.at(k);
    std::cout << "----- " << k << std::endl;
    print_state(state);
    std::cout << "\\\\\\\\\\\\" << std::endl;
    print_state(ground_truth.at(k));
  }
  std::cout << "Optimized g: " << solution.p.g_world.transpose() << std::endl;
  std::cout << "Optimized T_imu_from_vehicle: "
            << solution.p.T_imu_from_vehicle.translation().transpose() << "; "
            << solution.p.T_imu_from_vehicle.so3().log().transpose() << std::endl;

  draw_states(*geo, solution.x, false);

  view->spin_until_step();
}

}  // namespace jet_filter
}  // namespace estimation

int main() {
  // estimation::jet_filter::go();
  estimation::jet_filter::run_filter();
}<commit_msg>Stopping point for filter demo -- working<commit_after>#include "viewer/primitives/simple_geometry.hh"
#include "viewer/window_3d.hh"

#include "estimation/jet/jet_filter.hh"
#include "estimation/jet/jet_pose_opt.hh"

#include "estimation/jet/jet_rk4.hh"

#include "util/environment.hh"

#include "eigen.hh"
#include "sophus.hh"

namespace estimation {
namespace jet_filter {
namespace {

Parameters mock_parameters() {
  Parameters p;
  const jcc::Vec3 g(0.0, 0.0, -1.3);
  p.g_world = g;

  // const jcc::Vec3 t(0.1, 0.5, 0.1);
  const jcc::Vec3 t(0.0, 0.1, 0.25);
  // const jcc::Vec3 t = jcc::Vec3::Zero();
  const SO3 r_vehicle_from_sensor = SO3::exp(jcc::Vec3(0.0, 0.0, 0.0));
  const SE3 sensor_from_body(r_vehicle_from_sensor, t);
  p.T_imu_from_vehicle = sensor_from_body;
  return p;
}

void setup() {
  const auto view = viewer::get_window3d("Mr. Filter, filters");
  view->set_target_from_world(
      SE3(SO3::exp(Eigen::Vector3d(-3.1415 * 0.5, 0.0, 0.0)), Eigen::Vector3d::Zero()));
  view->set_continue_time_ms(10);

  const auto background = view->add_primitive<viewer::SimpleGeometry>();
  const geometry::shapes::Plane ground{jcc::Vec3::UnitZ(), 0.0};
  background->add_plane({ground});
  background->flip();
}

void draw_states(viewer::SimpleGeometry& geo,
                 const std::vector<State>& states,
                 bool truth) {
  const int n_states = static_cast<int>(states.size());
  for (int k = 0; k < n_states; ++k) {
    auto& state = states.at(k);
    const SE3 T_world_from_body = state.T_body_from_world.inverse();
    if (truth) {
      geo.add_axes({T_world_from_body, 0.1});
    } else {
      geo.add_axes({T_world_from_body, 0.05, 2.0, true});

      if (k < n_states - 1) {
        const auto& next_state = states.at(k + 1);
        const SE3 T_world_from_body_next = next_state.T_body_from_world.inverse();
        geo.add_line(
            {T_world_from_body.translation(), T_world_from_body_next.translation()});
      }
    }
  }
}

void print_state(const State& x) {
  std::cout << "\teps_dot   : " << x.eps_dot.transpose() << std::endl;
  std::cout << "\teps_ddot  : " << x.eps_ddot.transpose() << std::endl;
  std::cout << "\taccel_bias: " << x.accel_bias.transpose() << std::endl;
  std::cout << "\tgyro_bias : " << x.gyro_bias.transpose() << std::endl;
  // const auto res = observe_accel(xp.x, true_params);
  // std::cout << "\tExpected Measurement: " << res.transpose() << std::endl;
}

template <int dim, int row, int mat_size>
void set_diag_to_value(MatNd<mat_size, mat_size>& mat, double value) {
  mat.template block<dim, dim>(row, row) = (MatNd<dim, dim>::Identity() * value);
}
}  // namespace

class JetOptimizer {
 public:
  JetOptimizer() {
    MatNd<AccelMeasurement::DIM, AccelMeasurement::DIM> accel_cov;
    accel_cov.setZero();
    {
      set_diag_to_value<AccelMeasurementDelta::observed_acceleration_error_dim,
                        AccelMeasurementDelta::observed_acceleration_error_ind>(accel_cov,
                                                                                0.01);
    }

    MatNd<FiducialMeasurement::DIM, FiducialMeasurement::DIM> fiducial_cov;
    {
      fiducial_cov.block<3, 3>(0, 0) = MatNd<3, 3>::Identity() * 0.001;
      fiducial_cov.block<3, 3>(3, 3) = MatNd<3, 3>::Identity() * 0.0001;
    }
    MatNd<State::DIM, State::DIM> state_cov;
    {
      state_cov.setZero();
      set_diag_to_value<StateDelta::accel_bias_error_dim,
                        StateDelta::accel_bias_error_ind>(state_cov, 0.0001);
      set_diag_to_value<StateDelta::gyro_bias_error_dim, StateDelta::gyro_bias_error_ind>(
          state_cov, 0.0001);
      set_diag_to_value<StateDelta::eps_dot_error_dim, StateDelta::eps_dot_error_ind>(
          state_cov, 0.1);
      set_diag_to_value<StateDelta::eps_ddot_error_dim, StateDelta::eps_ddot_error_ind>(
          state_cov, 0.1);

      constexpr int T_error_dim = StateDelta::T_body_from_world_error_log_dim;
      constexpr int T_error_ind = StateDelta::T_body_from_world_error_log_ind;
      set_diag_to_value<T_error_dim, T_error_ind>(state_cov, 0.001);
    }

    const MatNd<GyroMeasurement::DIM, GyroMeasurement::DIM> gyro_cov = accel_cov;

    imu_id_ =
        pose_opt_.add_error_model<AccelMeasurement>(observe_accel_error_model, accel_cov);
    gyro_id_ =
        pose_opt_.add_error_model<GyroMeasurement>(observe_gyro_error_model, gyro_cov);

    fiducial_id_ = pose_opt_.add_error_model<FiducialMeasurement>(fiducial_error_model,
                                                                  fiducial_cov);
    pose_opt_.set_dynamics_cov(state_cov);
  }

  void measure_imu(const AccelMeasurement& meas, const TimePoint& t) {
    pose_opt_.add_measurement(meas, t, imu_id_);
  }

  void measure_fiducial(const FiducialMeasurement& meas, const TimePoint& t) {
    pose_opt_.add_measurement(meas, t, fiducial_id_);
  }

  void measure_gyro(const GyroMeasurement& meas, const TimePoint& t) {
    pose_opt_.add_measurement(meas, t, gyro_id_);
  }

  JetPoseOptimizer::Solution solve(const std::vector<State> x, const Parameters& p) {
    const auto view = viewer::get_window3d("Mr. Filter, filters");
    const auto geo = view->add_primitive<viewer::SimpleGeometry>();
    const auto visitor = [&view, &geo](const JetPoseOptimizer::Solution& soln) {
      geo->clear();
      draw_states(*geo, soln.x, false);
      geo->flip();
      std::cout << "\tOptimized g: " << soln.p.g_world.transpose() << std::endl;
      std::cout << "\tOptimized T_imu_from_vehicle: "
                << soln.p.T_imu_from_vehicle.translation().transpose() << "; "
                << soln.p.T_imu_from_vehicle.so3().log().transpose() << std::endl;
      view->spin_until_step();
    };

    return pose_opt_.solve({x, p}, visitor);
  }

 private:
  JetPoseOptimizer pose_opt_{rk4_integrate};
  int imu_id_ = -1;
  int gyro_id_ = -1;
  int fiducial_id_ = -1;
};

void run_filter() {
  const Parameters true_params = mock_parameters();

  JetOptimizer jet_opt;

  FilterState<State> xp0;
  {
    MatNd<State::DIM, State::DIM> state_cov;
    state_cov.setZero();
    set_diag_to_value<StateDelta::accel_bias_error_dim, StateDelta::accel_bias_error_ind>(
        state_cov, 0.0001);
    set_diag_to_value<StateDelta::gyro_bias_error_dim, StateDelta::gyro_bias_error_ind>(
        state_cov, 0.0001);
    set_diag_to_value<StateDelta::eps_dot_error_dim, StateDelta::eps_dot_error_ind>(
        state_cov, 0.01);
    set_diag_to_value<StateDelta::eps_ddot_error_dim, StateDelta::eps_ddot_error_ind>(
        state_cov, 0.1);

    xp0.P = state_cov;
  }
  // xp0.x.eps_dot[0] = 1.0;
  // xp0.x.eps_dot[4] = -0.6;
  // xp0.x.eps_dot[5] = -0.2;

  // xp0.x.eps_ddot[1] = 0.01;
  // xp0.x.eps_ddot[5] = 0.01;
  // xp0.x.eps_ddot[3] = 0.02;

  xp0.time_of_validity = {};

  State true_x = xp0.x;
  true_x.eps_dot[4] = 0.1;

  true_x.eps_dot[0] = 1.0;
  true_x.eps_dot[4] = -0.6;
  true_x.eps_dot[5] = -0.2;

  true_x.eps_ddot[1] = 0.01;
  true_x.eps_ddot[5] = 0.01;
  true_x.eps_ddot[3] = 0.02;

  JetFilter jf(xp0);

  // AccelMeasurement imu_meas;
  // imu_meas.observed_acceleration[0] = 2.0;

  setup();
  const auto view = viewer::get_window3d("Mr. Filter, filters");
  const auto obs_geo = view->add_primitive<viewer::SimpleGeometry>();
  constexpr double sphere_size_m = 0.05;

  std::vector<State> ground_truth;
  std::vector<State> est_states;

  TimePoint start_time = {};
  constexpr auto dt = to_duration(0.1);

  constexpr int NUM_SIM_STEPS = 300;

  TimePoint current_time = start_time;

  for (int k = 0; k < NUM_SIM_STEPS; ++k) {
    if (k > 75 && k < 130) {
      // true_x.eps_ddot[0] = 0.1;
      // true_x.eps_ddot[1] = -0.02;
      // true_x.eps_ddot[2] = -0.03;

      // true_x.eps_ddot[3] = 0.1;
      // true_x.eps_ddot[4] = 0.05;
      // true_x.eps_ddot[5] = -0.01;
    } else if (k > 130) {
      // true_x.eps_ddot.setZero();
    }

    //
    // Accelerometer Observation
    //
    // if ((k % 10 == 0) && k > 75) {
    if (true) {
      ground_truth.push_back(true_x);
      const AccelMeasurement imu_meas = observe_accel(true_x, true_params);
      std::cout << "Accel: " << imu_meas.observed_acceleration.transpose() << std::endl;

      jf.measure_imu(imu_meas, current_time);
      jet_opt.measure_imu(imu_meas, current_time);

      jf.free_run();
      est_states.push_back(jf.state().x);
      assert(jf.state().time_of_validity == current_time);

      const jcc::Vec4 accel_color(0.0, 0.7, 0.7, 0.8);
      obs_geo->add_sphere({jf.state().x.T_body_from_world.inverse().translation(),
                           sphere_size_m, accel_color});

      const SE3 world_from_body = jf.state().x.T_body_from_world.inverse();
      const jcc::Vec3 observed_accel_world_frame =
          (world_from_body.so3() * true_params.T_imu_from_vehicle.so3().inverse() *
           imu_meas.observed_acceleration);

      obs_geo->add_line(
          {world_from_body.translation(),
           world_from_body.translation() + (0.25 * observed_accel_world_frame),
           accel_color});
    }

    //
    // Gyro Observation
    //
    if (true) {
      constexpr auto dt2 = to_duration(0.01);
      true_x = rk4_integrate(true_x, true_params, to_seconds(dt2));
      current_time += dt2;
      ground_truth.push_back(true_x);

      const GyroMeasurement gyro_meas = observe_gyro(true_x, true_params);
      std::cout << "Gyro: " << gyro_meas.observed_w.transpose() << std::endl;

      jf.measure_gyro(gyro_meas, current_time);
      jet_opt.measure_gyro(gyro_meas, current_time);

      jf.free_run();
      est_states.push_back(jf.state().x);
      assert(jf.state().time_of_validity == current_time);

      const jcc::Vec4 gyro_color(0.7, 0.1, 0.7, 0.8);
      obs_geo->add_sphere({jf.state().x.T_body_from_world.inverse().translation(),
                           sphere_size_m, gyro_color});

      const SE3 world_from_body = jf.state().x.T_body_from_world.inverse();
      const jcc::Vec3 observed_w_world_frame =
          (world_from_body.so3() * true_params.T_imu_from_vehicle.so3().inverse() *
           gyro_meas.observed_w);

      obs_geo->add_line({world_from_body.translation(),
                         world_from_body.translation() + (0.25 * observed_w_world_frame),
                         gyro_color});
    }

    //
    // Fiducial Measurement
    //

    {
      constexpr auto dt2 = to_duration(0.05);
      true_x = rk4_integrate(true_x, true_params, to_seconds(dt2));
      current_time += dt2;

      ground_truth.push_back(true_x);
      const FiducialMeasurement fiducial_meas = observe_fiducial(true_x, true_params);

      jf.measure_fiducial(fiducial_meas, current_time);
      jet_opt.measure_fiducial(fiducial_meas, current_time);

      jf.free_run();
      est_states.push_back(jf.state().x);
      assert(jf.state().time_of_validity == current_time);
      obs_geo->add_sphere({jf.state().x.T_body_from_world.inverse().translation(),
                           sphere_size_m, jcc::Vec4(1.0, 0.0, 0.0, 1.0)});
    }

    //
    // Printout
    //
    {
      const auto xp = jf.state();
      std::cout << "k: " << k << std::endl;
      print_state(xp.x);
    }
    true_x = rk4_integrate(true_x, true_params, to_seconds(dt));
    current_time += dt;
  }

  // Do the optimization

  obs_geo->flip();
  const auto geo = view->add_primitive<viewer::SimpleGeometry>();
  draw_states(*geo, ground_truth, true);
  geo->flip();

  // const std::vector<State> crap_init(est_states.size(), xp0.x);
  // const auto solution = jet_opt.solve(crap_init, jf.parameters());
  const auto solution = jet_opt.solve(est_states, jf.parameters());
  // const auto solution = jet_opt.solve(ground_truth, jf.parameters());

  for (std::size_t k = 0; k < solution.x.size(); ++k) {
    const auto& state = solution.x.at(k);
    std::cout << "----- " << k << std::endl;
    print_state(state);
    std::cout << "\\\\\\\\\\\\" << std::endl;
    print_state(ground_truth.at(k));
  }
  std::cout << "Optimized g: " << solution.p.g_world.transpose() << std::endl;
  std::cout << "Optimized T_imu_from_vehicle: "
            << solution.p.T_imu_from_vehicle.translation().transpose() << "; "
            << solution.p.T_imu_from_vehicle.so3().log().transpose() << std::endl;

  draw_states(*geo, solution.x, false);

  view->spin_until_step();
}

}  // namespace jet_filter
}  // namespace estimation

int main() {
  // estimation::jet_filter::go();
  estimation::jet_filter::run_filter();
}<|endoftext|>


<commit_before>#include <cnoid/Plugin>
#include <boost/bind.hpp>

using namespace cnoid;
using namespace boost;

class ProtoPlugin : public Plugin
{
public:

    ProtoPlugin() : Plugin("ProtoPlugin")
    {

    }

	/* virtual functions can be overridden by child functions */
    virtual bool initialize()
    {
	// make things happen
    }

private:

    void onProtoTriggered()
    {
       	// make things binded to ProtoPlugin() happen
	// must return true (good way to test functionality)
    }
};

/* generate associated .cnoid file ( i think ) */
CNOID_IMPLEMENT_PLUGIN_ENTRY(ProtoPlugin)

<commit_msg>Update ProtoPlugin.cpp<commit_after>#include <cnoid/Plugin>
#include <boost/bind.hpp>

using namespace cnoid;
using namespace boost;

class ProtoPlugin : public Plugin
{
public:

    ProtoPlugin() : Plugin("ProtoPlugin")
    {

    }

	/* virtual functions can be overridden by child functions */
    virtual bool initialize()
    {
	// make things happen
    }

private:

    void onProtoTriggered()
    {
       	// make things binded to ProtoPlugin() happen

    }
};

/* generate associated .cnoid file ( i think ) */
CNOID_IMPLEMENT_PLUGIN_ENTRY(ProtoPlugin)

<|endoftext|>


<commit_before>#include <memory>

#include <archive.h>
#include <archive_entry.h>

#include "main.h"

static bool
care_about(struct archive_entry *entry)
{
	mode_t mode = archive_entry_mode(entry);

#if 0
	if (AE_IFLNK == (mode & AE_IFLNK)) {
		// ignoring symlinks for now
		return false;
	}
#endif

	if (AE_IFREG != (mode & AE_IFREG)) {
		// not a regular file
		return false;
	}

	if (!archive_entry_size_is_set(entry)) {
		// ignore files which have no size...
		return false;
	}

	return true;
}

// because isspace(3) is locale dependent
static bool
c_isspace(const char c) {
	return (c == ' '  || c == '\t' ||
	        c == '\n' || c == '\r' ||
	        c == '\v' || c == '\f');
}

static bool
ends_word(const char c) {
	return c_isspace(c) || c == '=';
}

// NOTE:
// Judgding from pacman/libalpm source code this function
// is way less strict about the formatting, as we skip whitespace
// between every word, whereas pacman matches /^(\w+) = (.*)$/ exactly.
static bool
read_info(Package *pkg, struct archive *tar, size_t size)
{
	std::vector<char> data(size);
	ssize_t rc = archive_read_data(tar, &data[0], size);
	if ((size_t)rc != size) {
		log(Error, "failed to read .PKGINFO");
		return false;
	}

	std::string str(&data[0], data.size());

	size_t pos = 0;
	auto skipwhite = [&]() {
		while (pos < size && c_isspace(str[pos]))
			++pos;
	};

	auto skipline = [&]() {
		while (pos < size && str[pos] != '\n')
			++pos;
	};

	auto getvalue = [&](const char *entryname, std::string &out) -> bool {
		skipwhite();
		if (str[pos] != '=') {
			log(Error, "Error in .PKGINFO");
			return false;
		}
		++pos;
		skipwhite();
		if (pos >= size) {
			log(Error, "invalid %s entry in .PKGINFO", entryname);
			return false;
		}
		size_t to = str.find_first_of(" \n\r\t", pos);
		out = str.substr(pos, to-pos);
		skipline();
		return true;
	};

	auto isentry = [&](const char *what, size_t len) -> bool {
		if (size-pos > len &&
		    str.compare(pos, len, what) == 0 &&
		    ends_word(str[pos+len]))
		{
			pos += len;
			return true;
		}
		return false;
	};

	std::string es;
	while (pos < size) {
		skipwhite();
		if (isentry("pkgname", sizeof("pkgname")-1)) {
			if (!getvalue("pkgname", pkg->name))
				return false;
			continue;
		}
		if (isentry("pkgver", sizeof("pkgver")-1)) {
			if (!getvalue("pkgver", pkg->version))
				return false;
			continue;
		}

		if (!opt_package_depends) {
			skipline();
			continue;
		}

		if (isentry("depend", sizeof("depend")-1)) {
			if (!getvalue("depend", es))
				return false;
			pkg->depends.push_back(es);
			continue;
		}
		if (isentry("optdepend", sizeof("optdepend")-1)) {
			if (!getvalue("optdepend", es))
				return false;
			es.erase(es.find_first_of(':'));
			if (es.length())
				pkg->optdepends.push_back(es);
			continue;
		}

		skipline();
	}
	return true;
}

static inline std::tuple<std::string, std::string>
splitpath(const std::string& path)
{
	size_t slash = path.find_last_of('/');
	if (slash == std::string::npos)
		return std::make_tuple("/", path);
	if (path[0] != '/')
		return std::make_tuple(std::move(std::string("/") + path.substr(0, slash)), path.substr(slash+1));
	return std::make_tuple(path.substr(0, slash), path.substr(slash+1));
}

static bool
read_object(Package *pkg, struct archive *tar, std::string &&filename, size_t size)
{
	std::vector<char> data;
	data.resize(size);

	ssize_t rc = archive_read_data(tar, &data[0], size);
	if (rc < 0) {
		log(Error, "failed to read from archive stream\n");
		return false;
	}
	else if ((size_t)rc != size) {
		log(Error, "file was short: %s\n", filename.c_str());
		return false;
	}

	bool err = false;
	rptr<Elf> object(Elf::open(&data[0], data.size(), &err, filename.c_str()));
	if (!object.get()) {
		if (err)
			log(Error, "error in: %s\n", filename.c_str());
		return !err;
	}

	auto split(std::move(splitpath(filename)));
	object->dirname  = std::move(std::get<0>(split));
	object->basename = std::move(std::get<1>(split));
	object->solve_paths(object->dirname);

	pkg->objects.push_back(object);

	return true;
}

static bool
add_entry(Package *pkg, struct archive *tar, struct archive_entry *entry)
{
	std::string filename(archive_entry_pathname(entry));
	bool isinfo = filename == ".PKGINFO";

	// for now we only care about files named lib.*\.so(\.|$)
	if (!isinfo && !care_about(entry))
	{
		archive_read_data_skip(tar);
		return true;
	}

	mode_t mode = archive_entry_mode(entry);
	if (AE_IFLNK == (mode & AE_IFLNK)) {
		// it's a symlink...
		const char *link = archive_entry_symlink(entry);
		if (!link) {
			log(Error, "error reading symlink");
			return false;
		}
		archive_read_data_skip(tar);
		pkg->load.symlinks[filename] = link;
		return true;
	}

	// Check the size
	size_t size = archive_entry_size(entry);
	if (!size)
		return true;

	if (isinfo)
		return read_info(pkg, tar, size);

	return read_object(pkg, tar, std::move(filename), size);
}

Elf*
Package::find(const std::string& dirname, const std::string& basename) const
{
	for (auto &obj : objects) {
		if (obj->dirname == dirname && obj->basename == basename)
			return const_cast<Elf*>(obj.get());
	}
	return nullptr;
}

void
Package::guess(const std::string& path)
{
	// extract the basename:
	size_t at = path.find_last_of('/');
	std::string base(at == std::string::npos ? path : path.substr(at+1));

	// at least N.tgz
	if (base.length() < 5)
		return;

	// ArchLinux scheme:
	// ${name}-${pkgver}-${pkgrel}-${CARCH}.pkg.tar.*
	// Slackware:
	// ${name}-${pkgver}-${CARCH}-${build}.t{gz,bz2,xz}

	// so the first part up to the first /-\d/ is part of the name
	size_t to = base.find_first_of("-.");

	// sanity:
	if (!to || to == std::string::npos)
		return;

	while (to+1 < base.length() && // gonna check [to+1]
	       base[to] != '.'      && // a dot ends the name
	       !(base[to+1] >= '0' && base[to+1] <= '9'))
	{
		// name can have dashes, let's move to the next one
		to = base.find_first_of("-.", to+1);
	}

	name = base.substr(0, to);
	if (base[to] != '-' || !(base[to+1] >= '0' && base[to+1] <= '9')) {
		// no version
		return;
	}

	// version
	size_t from = to+1;
	to = base.find_first_of('-', from);

	if (to == std::string::npos) {
		// we'll take it...
		version = base.substr(from);
		return;
	}

	bool slack = true;

	// check for a pkgrel (Arch scheme)
	if (base[to] == '-' &&
	    to+1 < base.length() &&
	    (base[to+1] >= '0' && base[to+1] <= '9'))
	{
		slack = false;
		to = base.find_first_of("-.", to+1);
	}

	version = base.substr(from, to-from);
	if (!slack || to == std::string::npos)
		return;

	// slackware build-name comes right before the extension
	to = base.find_last_of('.');
	if (!to || to == std::string::npos)
		return;
	from = base.find_last_of("-.", to-1);
	if (from && from != std::string::npos) {
		version.append(1, '-');
		version.append(base.substr(from+1, to-from-1));
	}
}

Package*
Package::open(const std::string& path)
{
	std::unique_ptr<Package> package(new Package);

	struct archive *tar = archive_read_new();
	archive_read_support_filter_all(tar);
	archive_read_support_format_all(tar);

	struct archive_entry *entry;
	if (ARCHIVE_OK != archive_read_open_filename(tar, path.c_str(), 10240)) {
		return 0;
	}

	while (ARCHIVE_OK == archive_read_next_header(tar, &entry)) {
		if (!add_entry(package.get(), tar, entry))
			return 0;
	}

	archive_read_free(tar);

	if (!package->name.length() && !package->version.length())
		package->guess(path);

	bool changed;
	do {
		changed = false;
		for (auto link = package->load.symlinks.begin(); link != package->load.symlinks.end();)
		{
			auto linkfrom = splitpath(link->first);
			decltype(linkfrom) linkto;

			// handle relative as well as absolute symlinks
			if (!link->second.length()) {
				// illegal
				++link;
				continue;
			}
			if (link->second[0] == '/') // absolute
				linkto = splitpath(link->second);
			else // relative
			{
				std::string fullpath = std::get<0>(linkfrom) + "/" + link->second;
				linkto = splitpath(fullpath);
			}

			Elf *obj = package->find(std::get<0>(linkto), std::get<1>(linkto));
			if (!obj) {
				++link;
				continue;
			}
			changed = true;

			Elf *copy = new Elf(*obj);
			copy->dirname  = std::move(std::get<0>(linkfrom));
			copy->basename = std::move(std::get<1>(linkfrom));
			copy->solve_paths(obj->dirname);

			package->objects.push_back(copy);
			package->load.symlinks.erase(link++);
		}
	} while (changed);
	package->load.symlinks.clear();

	return package.release();
}

void
Package::show_needed()
{
	const char *name = this->name.c_str();
	for (auto &obj : objects) {
		std::string path = obj->dirname + "/" + obj->basename;
		const char *objname = path.c_str();
		for (auto &need : obj->needed) {
			printf("%s: %s NEEDS %s\n", name, objname, need.c_str());
		}
	}
}
<commit_msg>fix a bug in reading of optdepend entries<commit_after>#include <memory>

#include <archive.h>
#include <archive_entry.h>

#include "main.h"

static bool
care_about(struct archive_entry *entry)
{
	mode_t mode = archive_entry_mode(entry);

#if 0
	if (AE_IFLNK == (mode & AE_IFLNK)) {
		// ignoring symlinks for now
		return false;
	}
#endif

	if (AE_IFREG != (mode & AE_IFREG)) {
		// not a regular file
		return false;
	}

	if (!archive_entry_size_is_set(entry)) {
		// ignore files which have no size...
		return false;
	}

	return true;
}

// because isspace(3) is locale dependent
static bool
c_isspace(const char c) {
	return (c == ' '  || c == '\t' ||
	        c == '\n' || c == '\r' ||
	        c == '\v' || c == '\f');
}

static bool
ends_word(const char c) {
	return c_isspace(c) || c == '=';
}

// NOTE:
// Judgding from pacman/libalpm source code this function
// is way less strict about the formatting, as we skip whitespace
// between every word, whereas pacman matches /^(\w+) = (.*)$/ exactly.
static bool
read_info(Package *pkg, struct archive *tar, const size_t size)
{
	std::vector<char> data(size);
	ssize_t rc = archive_read_data(tar, &data[0], size);
	if ((size_t)rc != size) {
		log(Error, "failed to read .PKGINFO");
		return false;
	}

	std::string str(&data[0], data.size());

	size_t pos = 0;
	auto skipwhite = [&]() {
		while (pos < size && c_isspace(str[pos]))
			++pos;
	};

	auto skipline = [&]() {
		while (pos < size && str[pos] != '\n')
			++pos;
	};

	auto getvalue = [&](const char *entryname, std::string &out) -> bool {
		skipwhite();
		if (pos >= size) {
			log(Error, "invalid %s entry in .PKGINFO", entryname);
			return false;
		}
		if (str[pos] != '=') {
			log(Error, "Error in .PKGINFO");
			return false;
		}
		++pos;
		skipwhite();
		if (pos >= size) {
			log(Error, "invalid %s entry in .PKGINFO", entryname);
			return false;
		}
		size_t to = str.find_first_of(" \n\r\t", pos);
		out = str.substr(pos, to-pos);
		skipline();
		return true;
	};

	auto isentry = [&](const char *what, size_t len) -> bool {
		if (size-pos > len &&
		    str.compare(pos, len, what) == 0 &&
		    ends_word(str[pos+len]))
		{
			pos += len;
			return true;
		}
		return false;
	};

	std::string es;
	while (pos < size) {
		skipwhite();
		if (isentry("pkgname", sizeof("pkgname")-1)) {
			if (!getvalue("pkgname", pkg->name))
				return false;
			continue;
		}
		if (isentry("pkgver", sizeof("pkgver")-1)) {
			if (!getvalue("pkgver", pkg->version))
				return false;
			continue;
		}

		if (!opt_package_depends) {
			skipline();
			continue;
		}

		if (isentry("depend", sizeof("depend")-1)) {
			if (!getvalue("depend", es))
				return false;
			pkg->depends.push_back(es);
			continue;
		}
		if (isentry("optdepend", sizeof("optdepend")-1)) {
			if (!getvalue("optdepend", es))
				return false;
			size_t c = es.find_first_of(':');
			if (c != std::string::npos)
				es.erase(c);
			if (es.length())
				pkg->optdepends.push_back(es);
			continue;
		}

		skipline();
	}
	return true;
}

static inline std::tuple<std::string, std::string>
splitpath(const std::string& path)
{
	size_t slash = path.find_last_of('/');
	if (slash == std::string::npos)
		return std::make_tuple("/", path);
	if (path[0] != '/')
		return std::make_tuple(std::move(std::string("/") + path.substr(0, slash)), path.substr(slash+1));
	return std::make_tuple(path.substr(0, slash), path.substr(slash+1));
}

static bool
read_object(Package *pkg, struct archive *tar, std::string &&filename, size_t size)
{
	std::vector<char> data;
	data.resize(size);

	ssize_t rc = archive_read_data(tar, &data[0], size);
	if (rc < 0) {
		log(Error, "failed to read from archive stream\n");
		return false;
	}
	else if ((size_t)rc != size) {
		log(Error, "file was short: %s\n", filename.c_str());
		return false;
	}

	bool err = false;
	rptr<Elf> object(Elf::open(&data[0], data.size(), &err, filename.c_str()));
	if (!object.get()) {
		if (err)
			log(Error, "error in: %s\n", filename.c_str());
		return !err;
	}

	auto split(std::move(splitpath(filename)));
	object->dirname  = std::move(std::get<0>(split));
	object->basename = std::move(std::get<1>(split));
	object->solve_paths(object->dirname);

	pkg->objects.push_back(object);

	return true;
}

static bool
add_entry(Package *pkg, struct archive *tar, struct archive_entry *entry)
{
	std::string filename(archive_entry_pathname(entry));
	bool isinfo = filename == ".PKGINFO";

	// for now we only care about files named lib.*\.so(\.|$)
	if (!isinfo && !care_about(entry))
	{
		archive_read_data_skip(tar);
		return true;
	}

	mode_t mode = archive_entry_mode(entry);
	if (AE_IFLNK == (mode & AE_IFLNK)) {
		// it's a symlink...
		const char *link = archive_entry_symlink(entry);
		if (!link) {
			log(Error, "error reading symlink");
			return false;
		}
		archive_read_data_skip(tar);
		pkg->load.symlinks[filename] = link;
		return true;
	}

	// Check the size
	size_t size = archive_entry_size(entry);
	if (!size)
		return true;

	if (isinfo)
		return read_info(pkg, tar, size);

	return read_object(pkg, tar, std::move(filename), size);
}

Elf*
Package::find(const std::string& dirname, const std::string& basename) const
{
	for (auto &obj : objects) {
		if (obj->dirname == dirname && obj->basename == basename)
			return const_cast<Elf*>(obj.get());
	}
	return nullptr;
}

void
Package::guess(const std::string& path)
{
	// extract the basename:
	size_t at = path.find_last_of('/');
	std::string base(at == std::string::npos ? path : path.substr(at+1));

	// at least N.tgz
	if (base.length() < 5)
		return;

	// ArchLinux scheme:
	// ${name}-${pkgver}-${pkgrel}-${CARCH}.pkg.tar.*
	// Slackware:
	// ${name}-${pkgver}-${CARCH}-${build}.t{gz,bz2,xz}

	// so the first part up to the first /-\d/ is part of the name
	size_t to = base.find_first_of("-.");

	// sanity:
	if (!to || to == std::string::npos)
		return;

	while (to+1 < base.length() && // gonna check [to+1]
	       base[to] != '.'      && // a dot ends the name
	       !(base[to+1] >= '0' && base[to+1] <= '9'))
	{
		// name can have dashes, let's move to the next one
		to = base.find_first_of("-.", to+1);
	}

	name = base.substr(0, to);
	if (base[to] != '-' || !(base[to+1] >= '0' && base[to+1] <= '9')) {
		// no version
		return;
	}

	// version
	size_t from = to+1;
	to = base.find_first_of('-', from);

	if (to == std::string::npos) {
		// we'll take it...
		version = base.substr(from);
		return;
	}

	bool slack = true;

	// check for a pkgrel (Arch scheme)
	if (base[to] == '-' &&
	    to+1 < base.length() &&
	    (base[to+1] >= '0' && base[to+1] <= '9'))
	{
		slack = false;
		to = base.find_first_of("-.", to+1);
	}

	version = base.substr(from, to-from);
	if (!slack || to == std::string::npos)
		return;

	// slackware build-name comes right before the extension
	to = base.find_last_of('.');
	if (!to || to == std::string::npos)
		return;
	from = base.find_last_of("-.", to-1);
	if (from && from != std::string::npos) {
		version.append(1, '-');
		version.append(base.substr(from+1, to-from-1));
	}
}

Package*
Package::open(const std::string& path)
{
	std::unique_ptr<Package> package(new Package);

	struct archive *tar = archive_read_new();
	archive_read_support_filter_all(tar);
	archive_read_support_format_all(tar);

	struct archive_entry *entry;
	if (ARCHIVE_OK != archive_read_open_filename(tar, path.c_str(), 10240)) {
		return 0;
	}

	while (ARCHIVE_OK == archive_read_next_header(tar, &entry)) {
		if (!add_entry(package.get(), tar, entry))
			return 0;
	}

	archive_read_free(tar);

	if (!package->name.length() && !package->version.length())
		package->guess(path);

	bool changed;
	do {
		changed = false;
		for (auto link = package->load.symlinks.begin(); link != package->load.symlinks.end();)
		{
			auto linkfrom = splitpath(link->first);
			decltype(linkfrom) linkto;

			// handle relative as well as absolute symlinks
			if (!link->second.length()) {
				// illegal
				++link;
				continue;
			}
			if (link->second[0] == '/') // absolute
				linkto = splitpath(link->second);
			else // relative
			{
				std::string fullpath = std::get<0>(linkfrom) + "/" + link->second;
				linkto = splitpath(fullpath);
			}

			Elf *obj = package->find(std::get<0>(linkto), std::get<1>(linkto));
			if (!obj) {
				++link;
				continue;
			}
			changed = true;

			Elf *copy = new Elf(*obj);
			copy->dirname  = std::move(std::get<0>(linkfrom));
			copy->basename = std::move(std::get<1>(linkfrom));
			copy->solve_paths(obj->dirname);

			package->objects.push_back(copy);
			package->load.symlinks.erase(link++);
		}
	} while (changed);
	package->load.symlinks.clear();

	return package.release();
}

void
Package::show_needed()
{
	const char *name = this->name.c_str();
	for (auto &obj : objects) {
		std::string path = obj->dirname + "/" + obj->basename;
		const char *objname = path.c_str();
		for (auto &need : obj->needed) {
			printf("%s: %s NEEDS %s\n", name, objname, need.c_str());
		}
	}
}
<|endoftext|>


<commit_before>// Copyright (c) 2013, German Neuroinformatics Node (G-Node)
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted under the terms of the BSD License. See
// LICENSE file in the root of the Project.

#ifndef NIX_PSIZE_H
#define NIX_PSIZE_H

#include <nix/Platform.hpp>
#include <nix/Exception.hpp>

#include <cstdint>
#include <stdexcept>
#include <algorithm>
#include <initializer_list>
#include <iostream>

namespace nix {

#ifdef _WIN32
//TODO: consider reimplementing NDSizeBase using std::vector (cf. issue #449)
#pragma warning(disable: 4996)
#endif

template<typename T>
class NDSizeBase {

public:

    typedef T        value_type;
    typedef T       *iterator;
    typedef const T *const_iterator;
    typedef T       &reference;
    typedef const T  const_reference;
    typedef T       *pointer;
    typedef size_t   difference_type;
    typedef size_t   size_type;

    NDSizeBase()
        : rank(0), dims(nullptr)
    {
    }

    explicit NDSizeBase(size_t rank)
        : rank(rank), dims(nullptr)
    {
        allocate();
    }

    explicit NDSizeBase(size_t rank, T fill_value)
        : rank(rank), dims(nullptr)
    {
        allocate();
        fill(fill_value);
    }

    template<typename U>
    NDSizeBase(std::initializer_list<U> args)
        : rank(args.size())
    {
        allocate();
        std::transform(args.begin(), args.end(), dims, [](const U& val) { return static_cast<T>(val);});
    }

    //copy
    NDSizeBase(const NDSizeBase &other)
        : rank(other.rank), dims(nullptr)
    {
        allocate();
        std::copy(other.dims, other.dims + rank, dims);
    }

    //move (not tested due to: http://llvm.org/bugs/show_bug.cgi?id=12208)
    NDSizeBase(NDSizeBase &&other)
        : rank(other.rank), dims(other.dims)
    {
        other.dims = nullptr;
        other.rank = 0;
    }

    //copy and move assignment operator (not tested, see above)
    NDSizeBase& operator=(NDSizeBase other) {
        swap(other);
        return *this;
    }

    // safe bool of the future (i.e. C++11)
    explicit operator bool() const {
        return rank > 0;
    }

    T& operator[] (const size_t index) {
        const NDSizeBase *this_const = const_cast<const NDSizeBase*>(this);
        return const_cast<T&>(this_const->operator[](index));
    }

    const T& operator[] (const size_t index) const {
        if (index + 1 > rank) {
            throw std::out_of_range ("Index out of bounds");
        }
        return dims[index];
    }

    NDSizeBase<T>& operator++() {
        std::for_each(begin(), end(), [](T &val) {
            val++;
        });
        return *this;
    }

    NDSizeBase<T> operator++(int) {
        NDSizeBase<T> snapshot(*this);
        operator++();
        return snapshot;
    }

    NDSizeBase<T>& operator+=(const NDSizeBase<T> &rhs) {
        if(size() != rhs.size()) {
            throw std::out_of_range (""); //fixme: use different exception
        }

        for (size_t i = 0; i < rank; i++) {
            dims[i] += rhs.dims[i];
        }

        return *this;
    }

    NDSizeBase<T>& operator+=(T val) {
        for (size_t i = 0; i < rank; i++) {
            dims[i] += val;
        }

        return *this;
    }

    NDSizeBase<T>& operator+=(int val) {
        return operator+=(static_cast<T>(val));
    }

    NDSizeBase<T>& operator--() {
        std::for_each(begin(), end(), [](T &val) {
            val--;
        });
        return *this;
    }

    NDSizeBase<T> operator--(int) {
        NDSizeBase<T> snapshot(*this);
        operator--();
        return snapshot;
    }

    NDSizeBase<T>& operator-=(const NDSizeBase<T> &rhs) {
        if(size() != rhs.size()) {
            throw std::out_of_range (""); //fixme: use different exception
        }

        for (size_t i = 0; i < rank; i++) {
            dims[i] -= rhs.dims[i];
        }

        return *this;
    }

    NDSizeBase<T>& operator-=(T val) {
        for (size_t i = 0; i < rank; i++) {
            dims[i] -= val;
        }

        return *this;
    }

    NDSizeBase<T>& operator-=(int val) {
        return operator-=(static_cast<T>(val));
    }

    void swap(NDSizeBase &other) {
        using std::swap;
        swap(dims, other.dims);
        rank = other.rank;
    }

    NDSizeBase<T>& operator*=(const NDSizeBase<T> &rhs) {
        if(size() != rhs.size()) {
            throw std::out_of_range (""); //fixme: use different exception
        }

        for (size_t i = 0; i < rank; i++) {
            dims[i] *= rhs.dims[i];
        }

        return *this;
    }

    NDSizeBase<T>& operator/=(const NDSizeBase<T> &rhs) {
        if(size() != rhs.size()) {
            throw std::out_of_range (""); //fixme: use different exception
        }

        for (size_t i = 0; i < rank; i++) {
            dims[i] /= rhs.dims[i];
        }

        return *this;
    }

    size_t size() const { return rank; }

    size_t nelms() const {
        T product = 1;
        std::for_each(begin(), end(), [&](T val) {
            product *= val;
        });
        //FIXME: check overflow before casting
        return static_cast<size_t>(product);
    }

    T dot(const NDSizeBase<T> &other) const {
        if(size() != other.size()) {
            throw std::out_of_range ("Dimensions do not match"); //fixme: use different exception
        }

        T res  = 0;
        for (size_t i = 0; i < rank; i++) {
            res += dims[i] * other.dims[i];
        }

        return res;
    }

    T* data() { return dims; }

    const T* data() const {return dims; }

    void fill(T value) {
        std::fill_n(dims, rank, value);
    }

    ~NDSizeBase() {
        delete[] dims;
    }

    //we are modelling a boost::Collection
    iterator begin() { return dims; }

    iterator end() { return dims + rank; }

    const_iterator begin() const { return dims; }

    const_iterator end() const { return dims + rank; }

    bool empty() const { return rank == 0; }

private:

    void allocate() {
        if (rank > 0) {
            dims = new T[rank];
        }
    }

    size_t   rank;
    T *dims;
};

template<typename T>
NDSizeBase<T> operator-(NDSizeBase<T> lhs, const NDSizeBase<T> &rhs)
{
    lhs -= rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator+(NDSizeBase<T> lhs, const NDSizeBase<T> &rhs)
{
    lhs += rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator+(NDSizeBase<T> lhs, T rhs)
{
    lhs += rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator+(T lhs, const NDSizeBase<T> &rhs)
{
    return operator+(rhs, lhs);
}

template<typename T>
NDSizeBase<T> operator+(NDSizeBase<T> lhs, int rhs)
{
    lhs += rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator+(int lhs, const NDSizeBase<T> &rhs)
{
    return operator+(rhs, lhs);
}

template<typename T>
NDSizeBase<T> operator*(NDSizeBase<T> lhs, const NDSizeBase<T> &rhs)
{
    lhs *= rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator*(NDSizeBase<T> lhs, T rhs)
{
    lhs *= rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator*(T lhs, const NDSizeBase<T> &rhs)
{
    return operator*(rhs, lhs);
}

template<typename T>
NDSizeBase<T> operator/(NDSizeBase<T> lhs, const NDSizeBase<T> &rhs)
{
    lhs /= rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator/(NDSizeBase<T> lhs, T rhs)
{
    lhs /= rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator/(T lhs, const NDSizeBase<T> &rhs)
{
    return operator/(rhs, lhs);
}

template<typename T>
inline bool operator==(const NDSizeBase<T> &lhs, const NDSizeBase<T> &rhs)
{
    if (lhs.size() != rhs.size())
        return false;

    for (size_t i = 0; i < lhs.size(); i++) {
        if (lhs[i] != rhs[i])
            return false;
    }

    return true;
}

template<typename T>
inline bool operator!=(const NDSizeBase<T> &lhs, const NDSizeBase<T> &rhs)
{
    return !operator==(lhs, rhs);
}

template<typename T>
inline bool operator<(const NDSizeBase<T> &lhs, const NDSizeBase<T> &rhs)
{
    if (lhs.size() != rhs.size()) {
        throw IncompatibleDimensions("size must agree to compare",
                                     "NDSizeBase < NDSizeBase ");
    }

    const size_t size = lhs.size();
    for (size_t i = 0; i < size; i++) {
        if (lhs[i] >= rhs[i]) {
            return false;
        }
    }

    return true;
}

template<typename T>
inline bool operator<=(const NDSizeBase<T> &lhs, const NDSizeBase<T> &rhs)
{
    if (lhs.size() != rhs.size()) {
        throw IncompatibleDimensions("size must agree to compare",
                                     "NDSizeBase < NDSizeBase ");
    }

    const size_t size = lhs.size();
    for (size_t i = 0; i < size; i++) {
        if (lhs[i] > rhs[i]) {
            return false;
        }
    }

    return true;
}

template<typename T>
inline bool operator>(const NDSizeBase<T> &lhs, const NDSizeBase<T> &rhs)
{
    return !(lhs <= rhs);
}

template<typename T>
inline bool operator>=(const NDSizeBase<T> &lhs, const NDSizeBase<T> &rhs)
{
    return !(lhs < rhs);
}

template<typename T>
inline std::ostream& operator<<(std::ostream &os, const NDSizeBase<T> &ndsize)
{
  os << "NDSize {";
  for(size_t i = 0; i < ndsize.size(); i++) {
    if (i != 0) {
      os << ", ";
    }

    os << ndsize[i];
  }

  os << "}\n";
  return os;
}

/* *****  */

//Ideally we would use unit64_t (and int64_t) here to directly specify
//the size we want, but for now we stick with how the hdf5 library
//defines hsize_t, otherwise we will run into issues when on plaforms
// where unit64_t is an incompatible type to the type of hsize_t
//(e.g. Ubuntu 12.04 LTS Server Edition 64 bit.)

typedef NDSizeBase<unsigned long long int>  NDSize;

typedef NDSizeBase<long long int> NDSSize;

} // namespace nix

#endif // NIX_PSIZE_H
<commit_msg>NDSize: define ndsize_t and ndssize_t<commit_after>// Copyright (c) 2013, German Neuroinformatics Node (G-Node)
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted under the terms of the BSD License. See
// LICENSE file in the root of the Project.

#ifndef NIX_PSIZE_H
#define NIX_PSIZE_H

#include <nix/Platform.hpp>
#include <nix/Exception.hpp>

#include <cstdint>
#include <stdexcept>
#include <algorithm>
#include <initializer_list>
#include <iostream>

namespace nix {

//Ideally we would use unit64_t (and int64_t) here to directly specify
//the size we want, but for now we stick with how the hdf5 library
//defines hsize_t, otherwise we will run into issues when on plaforms
// where unit64_t is an incompatible type to the type of hsize_t
//(e.g. Ubuntu 12.04 LTS Server Edition 64 bit.)

typedef unsigned long long int ndsize_t;
typedef long long int          ndssize_t;
#endif

template<typename T>
class NDSizeBase {

public:

    typedef T        value_type;
    typedef T       *iterator;
    typedef const T *const_iterator;
    typedef T       &reference;
    typedef const T  const_reference;
    typedef T       *pointer;
    typedef size_t   difference_type;
    typedef size_t   size_type;

    NDSizeBase()
        : rank(0), dims(nullptr)
    {
    }

    explicit NDSizeBase(size_t rank)
        : rank(rank), dims(nullptr)
    {
        allocate();
    }

    explicit NDSizeBase(size_t rank, T fill_value)
        : rank(rank), dims(nullptr)
    {
        allocate();
        fill(fill_value);
    }

    template<typename U>
    NDSizeBase(std::initializer_list<U> args)
        : rank(args.size())
    {
        allocate();
        std::transform(args.begin(), args.end(), dims, [](const U& val) { return static_cast<T>(val);});
    }

    //copy
    NDSizeBase(const NDSizeBase &other)
        : rank(other.rank), dims(nullptr)
    {
        allocate();
        std::copy(other.dims, other.dims + rank, dims);
    }

    //move (not tested due to: http://llvm.org/bugs/show_bug.cgi?id=12208)
    NDSizeBase(NDSizeBase &&other)
        : rank(other.rank), dims(other.dims)
    {
        other.dims = nullptr;
        other.rank = 0;
    }

    //copy and move assignment operator (not tested, see above)
    NDSizeBase& operator=(NDSizeBase other) {
        swap(other);
        return *this;
    }

    // safe bool of the future (i.e. C++11)
    explicit operator bool() const {
        return rank > 0;
    }

    T& operator[] (const size_t index) {
        const NDSizeBase *this_const = const_cast<const NDSizeBase*>(this);
        return const_cast<T&>(this_const->operator[](index));
    }

    const T& operator[] (const size_t index) const {
        if (index + 1 > rank) {
            throw std::out_of_range ("Index out of bounds");
        }
        return dims[index];
    }

    NDSizeBase<T>& operator++() {
        std::for_each(begin(), end(), [](T &val) {
            val++;
        });
        return *this;
    }

    NDSizeBase<T> operator++(int) {
        NDSizeBase<T> snapshot(*this);
        operator++();
        return snapshot;
    }

    NDSizeBase<T>& operator+=(const NDSizeBase<T> &rhs) {
        if(size() != rhs.size()) {
            throw std::out_of_range (""); //fixme: use different exception
        }

        for (size_t i = 0; i < rank; i++) {
            dims[i] += rhs.dims[i];
        }

        return *this;
    }

    NDSizeBase<T>& operator+=(T val) {
        for (size_t i = 0; i < rank; i++) {
            dims[i] += val;
        }

        return *this;
    }

    NDSizeBase<T>& operator+=(int val) {
        return operator+=(static_cast<T>(val));
    }

    NDSizeBase<T>& operator--() {
        std::for_each(begin(), end(), [](T &val) {
            val--;
        });
        return *this;
    }

    NDSizeBase<T> operator--(int) {
        NDSizeBase<T> snapshot(*this);
        operator--();
        return snapshot;
    }

    NDSizeBase<T>& operator-=(const NDSizeBase<T> &rhs) {
        if(size() != rhs.size()) {
            throw std::out_of_range (""); //fixme: use different exception
        }

        for (size_t i = 0; i < rank; i++) {
            dims[i] -= rhs.dims[i];
        }

        return *this;
    }

    NDSizeBase<T>& operator-=(T val) {
        for (size_t i = 0; i < rank; i++) {
            dims[i] -= val;
        }

        return *this;
    }

    NDSizeBase<T>& operator-=(int val) {
        return operator-=(static_cast<T>(val));
    }

    void swap(NDSizeBase &other) {
        using std::swap;
        swap(dims, other.dims);
        rank = other.rank;
    }

    NDSizeBase<T>& operator*=(const NDSizeBase<T> &rhs) {
        if(size() != rhs.size()) {
            throw std::out_of_range (""); //fixme: use different exception
        }

        for (size_t i = 0; i < rank; i++) {
            dims[i] *= rhs.dims[i];
        }

        return *this;
    }

    NDSizeBase<T>& operator/=(const NDSizeBase<T> &rhs) {
        if(size() != rhs.size()) {
            throw std::out_of_range (""); //fixme: use different exception
        }

        for (size_t i = 0; i < rank; i++) {
            dims[i] /= rhs.dims[i];
        }

        return *this;
    }

    size_t size() const { return rank; }

    size_t nelms() const {
        T product = 1;
        std::for_each(begin(), end(), [&](T val) {
            product *= val;
        });
        //FIXME: check overflow before casting
        return static_cast<size_t>(product);
    }

    T dot(const NDSizeBase<T> &other) const {
        if(size() != other.size()) {
            throw std::out_of_range ("Dimensions do not match"); //fixme: use different exception
        }

        T res  = 0;
        for (size_t i = 0; i < rank; i++) {
            res += dims[i] * other.dims[i];
        }

        return res;
    }

    T* data() { return dims; }

    const T* data() const {return dims; }

    void fill(T value) {
        std::fill_n(dims, rank, value);
    }

    ~NDSizeBase() {
        delete[] dims;
    }

    //we are modelling a boost::Collection
    iterator begin() { return dims; }

    iterator end() { return dims + rank; }

    const_iterator begin() const { return dims; }

    const_iterator end() const { return dims + rank; }

    bool empty() const { return rank == 0; }

private:

    void allocate() {
        if (rank > 0) {
            dims = new T[rank];
        }
    }

    size_t   rank;
    T *dims;
};

template<typename T>
NDSizeBase<T> operator-(NDSizeBase<T> lhs, const NDSizeBase<T> &rhs)
{
    lhs -= rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator+(NDSizeBase<T> lhs, const NDSizeBase<T> &rhs)
{
    lhs += rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator+(NDSizeBase<T> lhs, T rhs)
{
    lhs += rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator+(T lhs, const NDSizeBase<T> &rhs)
{
    return operator+(rhs, lhs);
}

template<typename T>
NDSizeBase<T> operator+(NDSizeBase<T> lhs, int rhs)
{
    lhs += rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator+(int lhs, const NDSizeBase<T> &rhs)
{
    return operator+(rhs, lhs);
}

template<typename T>
NDSizeBase<T> operator*(NDSizeBase<T> lhs, const NDSizeBase<T> &rhs)
{
    lhs *= rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator*(NDSizeBase<T> lhs, T rhs)
{
    lhs *= rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator*(T lhs, const NDSizeBase<T> &rhs)
{
    return operator*(rhs, lhs);
}

template<typename T>
NDSizeBase<T> operator/(NDSizeBase<T> lhs, const NDSizeBase<T> &rhs)
{
    lhs /= rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator/(NDSizeBase<T> lhs, T rhs)
{
    lhs /= rhs;
    return lhs;
}

template<typename T>
NDSizeBase<T> operator/(T lhs, const NDSizeBase<T> &rhs)
{
    return operator/(rhs, lhs);
}

template<typename T>
inline bool operator==(const NDSizeBase<T> &lhs, const NDSizeBase<T> &rhs)
{
    if (lhs.size() != rhs.size())
        return false;

    for (size_t i = 0; i < lhs.size(); i++) {
        if (lhs[i] != rhs[i])
            return false;
    }

    return true;
}

template<typename T>
inline bool operator!=(const NDSizeBase<T> &lhs, const NDSizeBase<T> &rhs)
{
    return !operator==(lhs, rhs);
}

template<typename T>
inline bool operator<(const NDSizeBase<T> &lhs, const NDSizeBase<T> &rhs)
{
    if (lhs.size() != rhs.size()) {
        throw IncompatibleDimensions("size must agree to compare",
                                     "NDSizeBase < NDSizeBase ");
    }

    const size_t size = lhs.size();
    for (size_t i = 0; i < size; i++) {
        if (lhs[i] >= rhs[i]) {
            return false;
        }
    }

    return true;
}

template<typename T>
inline bool operator<=(const NDSizeBase<T> &lhs, const NDSizeBase<T> &rhs)
{
    if (lhs.size() != rhs.size()) {
        throw IncompatibleDimensions("size must agree to compare",
                                     "NDSizeBase < NDSizeBase ");
    }

    const size_t size = lhs.size();
    for (size_t i = 0; i < size; i++) {
        if (lhs[i] > rhs[i]) {
            return false;
        }
    }

    return true;
}

template<typename T>
inline bool operator>(const NDSizeBase<T> &lhs, const NDSizeBase<T> &rhs)
{
    return !(lhs <= rhs);
}

template<typename T>
inline bool operator>=(const NDSizeBase<T> &lhs, const NDSizeBase<T> &rhs)
{
    return !(lhs < rhs);
}

template<typename T>
inline std::ostream& operator<<(std::ostream &os, const NDSizeBase<T> &ndsize)
{
  os << "NDSize {";
  for(size_t i = 0; i < ndsize.size(); i++) {
    if (i != 0) {
      os << ", ";
    }

    os << ndsize[i];
  }

  os << "}\n";
  return os;
}

/* *****  */

//Ideally we would use unit64_t (and int64_t) here to directly specify
//the size we want, but for now we stick with how the hdf5 library
//defines hsize_t, otherwise we will run into issues when on plaforms
// where unit64_t is an incompatible type to the type of hsize_t
//(e.g. Ubuntu 12.04 LTS Server Edition 64 bit.)

typedef NDSizeBase<unsigned long long int>  NDSize;

typedef NDSizeBase<long long int> NDSSize;

} // namespace nix

#endif // NIX_PSIZE_H
<|endoftext|>


<commit_before>
/*
 * Copyright 2006 The Android Open Source Project
 *
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include "SkComposeShader.h"
#include "SkColorFilter.h"
#include "SkColorPriv.h"
#include "SkXfermode.h"

///////////////////////////////////////////////////////////////////////////////

SkComposeShader::SkComposeShader(SkShader* sA, SkShader* sB, SkXfermode* mode) {
    fShaderA = sA;  sA->ref();
    fShaderB = sB;  sB->ref();
    // mode may be null
    fMode = mode;
    SkSafeRef(mode);
}

SkComposeShader::SkComposeShader(SkFlattenableReadBuffer& buffer) :
    INHERITED(buffer) {
    fShaderA = static_cast<SkShader*>(buffer.readFlattenable());
    fShaderB = static_cast<SkShader*>(buffer.readFlattenable());
    fMode = static_cast<SkXfermode*>(buffer.readFlattenable());
}

SkComposeShader::~SkComposeShader() {
    SkSafeUnref(fMode);
    fShaderB->unref();
    fShaderA->unref();
}

void SkComposeShader::beginSession() {
    this->INHERITED::beginSession();
    fShaderA->beginSession();
    fShaderB->beginSession();
}

void SkComposeShader::endSession() {
    fShaderA->endSession();
    fShaderB->endSession();
    this->INHERITED::endSession();
}

class SkAutoAlphaRestore {
public:
    SkAutoAlphaRestore(SkPaint* paint, uint8_t newAlpha) {
        fAlpha = paint->getAlpha();
        fPaint = paint;
        paint->setAlpha(newAlpha);
    }

    ~SkAutoAlphaRestore() {
        fPaint->setAlpha(fAlpha);
    }
private:
    SkPaint*    fPaint;
    uint8_t     fAlpha;
};

void SkComposeShader::flatten(SkFlattenableWriteBuffer& buffer) {
    this->INHERITED::flatten(buffer);
    buffer.writeFlattenable(fShaderA);
    buffer.writeFlattenable(fShaderB);
    buffer.writeFlattenable(fMode);
}

/*  We call setContext on our two worker shaders. However, we
    always let them see opaque alpha, and if the paint really
    is translucent, then we apply that after the fact.
*/
bool SkComposeShader::setContext(const SkBitmap& device,
                                 const SkPaint& paint,
                                 const SkMatrix& matrix) {
    if (!this->INHERITED::setContext(device, paint, matrix)) {
        return false;
    }

    // we preconcat our localMatrix (if any) with the device matrix
    // before calling our sub-shaders

    SkMatrix tmpM;

    (void)this->getLocalMatrix(&tmpM);
    tmpM.setConcat(matrix, tmpM);

    SkAutoAlphaRestore  restore(const_cast<SkPaint*>(&paint), 0xFF);

    return  fShaderA->setContext(device, paint, tmpM) &&
            fShaderB->setContext(device, paint, tmpM);
}

// larger is better (fewer times we have to loop), but we shouldn't
// take up too much stack-space (each element is 4 bytes)
#define TMP_COLOR_COUNT     64

void SkComposeShader::shadeSpan(int x, int y, SkPMColor result[], int count) {
    SkShader*   shaderA = fShaderA;
    SkShader*   shaderB = fShaderB;
    SkXfermode* mode = fMode;
    unsigned    scale = SkAlpha255To256(this->getPaintAlpha());

    SkPMColor   tmp[TMP_COLOR_COUNT];

    if (NULL == mode) {   // implied SRC_OVER
        // TODO: when we have a good test-case, should use SkBlitRow::Proc32
        // for these loops
        do {
            int n = count;
            if (n > TMP_COLOR_COUNT) {
                n = TMP_COLOR_COUNT;
            }

            shaderA->shadeSpan(x, y, result, n);
            shaderB->shadeSpan(x, y, tmp, n);

            if (256 == scale) {
                for (int i = 0; i < n; i++) {
                    result[i] = SkPMSrcOver(tmp[i], result[i]);
                }
            } else {
                for (int i = 0; i < n; i++) {
                    result[i] = SkAlphaMulQ(SkPMSrcOver(tmp[i], result[i]),
                                            scale);
                }
            }

            result += n;
            x += n;
            count -= n;
        } while (count > 0);
    } else {    // use mode for the composition
        do {
            int n = count;
            if (n > TMP_COLOR_COUNT) {
                n = TMP_COLOR_COUNT;
            }

            shaderA->shadeSpan(x, y, result, n);
            shaderB->shadeSpan(x, y, tmp, n);
            mode->xfer32(result, tmp, n, NULL);

            if (256 == scale) {
                for (int i = 0; i < n; i++) {
                    result[i] = SkAlphaMulQ(result[i], scale);
                }
            }

            result += n;
            x += n;
            count -= n;
        } while (count > 0);
    }
}

<commit_msg>handle if unflattening returned a null shader<commit_after>
/*
 * Copyright 2006 The Android Open Source Project
 *
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include "SkComposeShader.h"
#include "SkColorFilter.h"
#include "SkColorPriv.h"
#include "SkColorShader.h"
#include "SkXfermode.h"

///////////////////////////////////////////////////////////////////////////////

SkComposeShader::SkComposeShader(SkShader* sA, SkShader* sB, SkXfermode* mode) {
    fShaderA = sA;  sA->ref();
    fShaderB = sB;  sB->ref();
    // mode may be null
    fMode = mode;
    SkSafeRef(mode);
}

SkComposeShader::SkComposeShader(SkFlattenableReadBuffer& buffer) :
    INHERITED(buffer) {
    fShaderA = static_cast<SkShader*>(buffer.readFlattenable());
    if (NULL == fShaderA) {
        fShaderA = SkNEW_ARGS(SkColorShader, (0));
    }
    fShaderB = static_cast<SkShader*>(buffer.readFlattenable());
    if (NULL == fShaderB) {
        fShaderB = SkNEW_ARGS(SkColorShader, (0));
    }
    fMode = static_cast<SkXfermode*>(buffer.readFlattenable());
}

SkComposeShader::~SkComposeShader() {
    SkSafeUnref(fMode);
    fShaderB->unref();
    fShaderA->unref();
}

void SkComposeShader::beginSession() {
    this->INHERITED::beginSession();
    fShaderA->beginSession();
    fShaderB->beginSession();
}

void SkComposeShader::endSession() {
    fShaderA->endSession();
    fShaderB->endSession();
    this->INHERITED::endSession();
}

class SkAutoAlphaRestore {
public:
    SkAutoAlphaRestore(SkPaint* paint, uint8_t newAlpha) {
        fAlpha = paint->getAlpha();
        fPaint = paint;
        paint->setAlpha(newAlpha);
    }

    ~SkAutoAlphaRestore() {
        fPaint->setAlpha(fAlpha);
    }
private:
    SkPaint*    fPaint;
    uint8_t     fAlpha;
};

void SkComposeShader::flatten(SkFlattenableWriteBuffer& buffer) {
    this->INHERITED::flatten(buffer);
    buffer.writeFlattenable(fShaderA);
    buffer.writeFlattenable(fShaderB);
    buffer.writeFlattenable(fMode);
}

/*  We call setContext on our two worker shaders. However, we
    always let them see opaque alpha, and if the paint really
    is translucent, then we apply that after the fact.
*/
bool SkComposeShader::setContext(const SkBitmap& device,
                                 const SkPaint& paint,
                                 const SkMatrix& matrix) {
    if (!this->INHERITED::setContext(device, paint, matrix)) {
        return false;
    }

    // we preconcat our localMatrix (if any) with the device matrix
    // before calling our sub-shaders

    SkMatrix tmpM;

    (void)this->getLocalMatrix(&tmpM);
    tmpM.setConcat(matrix, tmpM);

    SkAutoAlphaRestore  restore(const_cast<SkPaint*>(&paint), 0xFF);

    return  fShaderA->setContext(device, paint, tmpM) &&
            fShaderB->setContext(device, paint, tmpM);
}

// larger is better (fewer times we have to loop), but we shouldn't
// take up too much stack-space (each element is 4 bytes)
#define TMP_COLOR_COUNT     64

void SkComposeShader::shadeSpan(int x, int y, SkPMColor result[], int count) {
    SkShader*   shaderA = fShaderA;
    SkShader*   shaderB = fShaderB;
    SkXfermode* mode = fMode;
    unsigned    scale = SkAlpha255To256(this->getPaintAlpha());

    SkPMColor   tmp[TMP_COLOR_COUNT];

    if (NULL == mode) {   // implied SRC_OVER
        // TODO: when we have a good test-case, should use SkBlitRow::Proc32
        // for these loops
        do {
            int n = count;
            if (n > TMP_COLOR_COUNT) {
                n = TMP_COLOR_COUNT;
            }

            shaderA->shadeSpan(x, y, result, n);
            shaderB->shadeSpan(x, y, tmp, n);

            if (256 == scale) {
                for (int i = 0; i < n; i++) {
                    result[i] = SkPMSrcOver(tmp[i], result[i]);
                }
            } else {
                for (int i = 0; i < n; i++) {
                    result[i] = SkAlphaMulQ(SkPMSrcOver(tmp[i], result[i]),
                                            scale);
                }
            }

            result += n;
            x += n;
            count -= n;
        } while (count > 0);
    } else {    // use mode for the composition
        do {
            int n = count;
            if (n > TMP_COLOR_COUNT) {
                n = TMP_COLOR_COUNT;
            }

            shaderA->shadeSpan(x, y, result, n);
            shaderB->shadeSpan(x, y, tmp, n);
            mode->xfer32(result, tmp, n, NULL);

            if (256 == scale) {
                for (int i = 0; i < n; i++) {
                    result[i] = SkAlphaMulQ(result[i], scale);
                }
            }

            result += n;
            x += n;
            count -= n;
        } while (count > 0);
    }
}

<|endoftext|>