<commit_before>// Copyright (c) 2010 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 "chrome/browser/browser_main.h"
#include "chrome/browser/browser_main_win.h"

#include <windows.h>
#include <shellapi.h>

#include <algorithm>

#include "app/l10n_util.h"
#include "app/win_util.h"
#include "base/command_line.h"
#include "base/environment.h"
#include "base/i18n/rtl.h"
#include "base/nss_util.h"
#include "base/path_service.h"
#include "base/scoped_ptr.h"
#include "base/utf_string_conversions.h"
#include "base/win_util.h"
#include "chrome/browser/browser_list.h"
#include "chrome/browser/first_run/first_run.h"
#include "chrome/browser/metrics/metrics_service.h"
#include "chrome/browser/views/uninstall_view.h"
#include "chrome/common/chrome_switches.h"
#include "chrome/common/env_vars.h"
#include "chrome/common/result_codes.h"
#include "chrome/installer/util/helper.h"
#include "chrome/installer/util/install_util.h"
#include "chrome/installer/util/shell_util.h"
#include "grit/chromium_strings.h"
#include "grit/generated_resources.h"
#include "net/base/winsock_init.h"
#include "net/socket/ssl_client_socket_nss_factory.h"
#include "views/focus/accelerator_handler.h"
#include "views/window/window.h"

void DidEndMainMessageLoop() {
  OleUninitialize();
}

void RecordBreakpadStatusUMA(MetricsService* metrics) {
  DWORD len = ::GetEnvironmentVariableW(
      ASCIIToWide(env_vars::kNoOOBreakpad).c_str() , NULL, 0);
  metrics->RecordBreakpadRegistration((len == 0));
  metrics->RecordBreakpadHasDebugger(TRUE == ::IsDebuggerPresent());
}

void WarnAboutMinimumSystemRequirements() {
  if (win_util::GetWinVersion() < win_util::WINVERSION_XP) {
    // Display a warning message if the user is running chrome on Windows 2000.
    const std::wstring text =
        l10n_util::GetString(IDS_UNSUPPORTED_OS_PRE_WIN_XP);
    const std::wstring caption = l10n_util::GetString(IDS_PRODUCT_NAME);
    win_util::MessageBox(NULL, text, caption,
                         MB_OK | MB_ICONWARNING | MB_TOPMOST);
  }
}

int AskForUninstallConfirmation() {
  int ret = ResultCodes::NORMAL_EXIT;
  views::Window::CreateChromeWindow(NULL, gfx::Rect(),
                                    new UninstallView(ret))->Show();
  views::AcceleratorHandler accelerator_handler;
  MessageLoopForUI::current()->Run(&accelerator_handler);
  return ret;
}

void ShowCloseBrowserFirstMessageBox() {
  const std::wstring text = l10n_util::GetString(IDS_UNINSTALL_CLOSE_APP);
  const std::wstring caption = l10n_util::GetString(IDS_PRODUCT_NAME);
  const UINT flags = MB_OK | MB_ICONWARNING | MB_TOPMOST;
  win_util::MessageBox(NULL, text, caption, flags);
}

int DoUninstallTasks(bool chrome_still_running) {
  // We want to show a warning to user (and exit) if Chrome is already running
  // *before* we show the uninstall confirmation dialog box. But while the
  // uninstall confirmation dialog is up, user might start Chrome, so we
  // check once again after user acknowledges Uninstall dialog.
  if (chrome_still_running) {
    ShowCloseBrowserFirstMessageBox();
    return ResultCodes::UNINSTALL_CHROME_ALIVE;
  }
  int ret = AskForUninstallConfirmation();
  if (Upgrade::IsBrowserAlreadyRunning()) {
    ShowCloseBrowserFirstMessageBox();
    return ResultCodes::UNINSTALL_CHROME_ALIVE;
  }

  if (ret != ResultCodes::UNINSTALL_USER_CANCEL) {
    // The following actions are just best effort.
    LOG(INFO) << "Executing uninstall actions";
    if (!FirstRun::RemoveSentinel())
      LOG(INFO) << "Failed to delete sentinel file.";
    // We want to remove user level shortcuts and we only care about the ones
    // created by us and not by the installer so |alternate| is false.
    if (!ShellUtil::RemoveChromeDesktopShortcut(ShellUtil::CURRENT_USER, false))
      LOG(INFO) << "Failed to delete desktop shortcut.";
    if (!ShellUtil::RemoveChromeQuickLaunchShortcut(ShellUtil::CURRENT_USER))
      LOG(INFO) << "Failed to delete quick launch shortcut.";
  }
  return ret;
}

// Prepares the localized strings that are going to be displayed to
// the user if the browser process dies. These strings are stored in the
// environment block so they are accessible in the early stages of the
// chrome executable's lifetime.
void PrepareRestartOnCrashEnviroment(const CommandLine &parsed_command_line) {
  // Clear this var so child processes don't show the dialog by default.
  scoped_ptr<base::Environment> env(base::Environment::Create());
  env->UnSetVar(env_vars::kShowRestart);

  // For non-interactive tests we don't restart on crash.
  if (env->HasVar(env_vars::kHeadless))
    return;

  // If the known command-line test options are used we don't create the
  // environment block which means we don't get the restart dialog.
  if (parsed_command_line.HasSwitch(switches::kBrowserCrashTest) ||
      parsed_command_line.HasSwitch(switches::kBrowserAssertTest) ||
      parsed_command_line.HasSwitch(switches::kNoErrorDialogs))
    return;

  // The encoding we use for the info is "title|context|direction" where
  // direction is either env_vars::kRtlLocale or env_vars::kLtrLocale depending
  // on the current locale.
  string16 dlg_strings(l10n_util::GetStringUTF16(IDS_CRASH_RECOVERY_TITLE));
  dlg_strings.push_back('|');
  string16 adjusted_string;
  base::i18n::AdjustStringForLocaleDirection(
      l10n_util::GetStringUTF16(IDS_CRASH_RECOVERY_CONTENT), &adjusted_string);
  dlg_strings.append(adjusted_string);
  dlg_strings.push_back('|');
  dlg_strings.append(ASCIIToUTF16(
      base::i18n::IsRTL() ? env_vars::kRtlLocale : env_vars::kLtrLocale));

  env->SetVar(env_vars::kRestartInfo, UTF16ToUTF8(dlg_strings));
}

// This method handles the --hide-icons and --show-icons command line options
// for chrome that get triggered by Windows from registry entries
// HideIconsCommand & ShowIconsCommand. Chrome doesn't support hide icons
// functionality so we just ask the users if they want to uninstall Chrome.
int HandleIconsCommands(const CommandLine &parsed_command_line) {
  if (parsed_command_line.HasSwitch(switches::kHideIcons)) {
    std::wstring cp_applet;
    win_util::WinVersion version = win_util::GetWinVersion();
    if (version >= win_util::WINVERSION_VISTA) {
      cp_applet.assign(L"Programs and Features");  // Windows Vista and later.
    } else if (version >= win_util::WINVERSION_XP) {
      cp_applet.assign(L"Add/Remove Programs");  // Windows XP.
    } else {
      return ResultCodes::UNSUPPORTED_PARAM;  // Not supported
    }

    const std::wstring msg = l10n_util::GetStringF(IDS_HIDE_ICONS_NOT_SUPPORTED,
                                                   cp_applet);
    const std::wstring caption = l10n_util::GetString(IDS_PRODUCT_NAME);
    const UINT flags = MB_OKCANCEL | MB_ICONWARNING | MB_TOPMOST;
    if (IDOK == win_util::MessageBox(NULL, msg, caption, flags))
      ShellExecute(NULL, NULL, L"appwiz.cpl", NULL, NULL, SW_SHOWNORMAL);
    return ResultCodes::NORMAL_EXIT;  // Exit as we are not launching browser.
  }
  // We don't hide icons so we shouldn't do anything special to show them
  return ResultCodes::UNSUPPORTED_PARAM;
}

// Check if there is any machine level Chrome installed on the current
// machine. If yes and the current Chrome process is user level, we do not
// allow the user level Chrome to run. So we notify the user and uninstall
// user level Chrome.
bool CheckMachineLevelInstall() {
  scoped_ptr<installer::Version> version(InstallUtil::GetChromeVersion(true));
  if (version.get()) {
    std::wstring exe;
    PathService::Get(base::DIR_EXE, &exe);
    std::transform(exe.begin(), exe.end(), exe.begin(), tolower);
    std::wstring user_exe_path = installer::GetChromeInstallPath(false);
    std::transform(user_exe_path.begin(), user_exe_path.end(),
                   user_exe_path.begin(), tolower);
    if (exe == user_exe_path) {
      const std::wstring text =
          l10n_util::GetString(IDS_MACHINE_LEVEL_INSTALL_CONFLICT);
      const std::wstring caption = l10n_util::GetString(IDS_PRODUCT_NAME);
      const UINT flags = MB_OK | MB_ICONERROR | MB_TOPMOST;
      win_util::MessageBox(NULL, text, caption, flags);
      std::wstring uninstall_cmd = InstallUtil::GetChromeUninstallCmd(false);
      if (!uninstall_cmd.empty()) {
        uninstall_cmd.append(L" --");
        uninstall_cmd.append(installer_util::switches::kForceUninstall);
        uninstall_cmd.append(L" --");
        uninstall_cmd.append(installer_util::switches::kDoNotRemoveSharedItems);
        base::LaunchApp(uninstall_cmd, false, false, NULL);
      }
      return true;
    }
  }
  return false;
}

// BrowserMainPartsWin ---------------------------------------------------------

class BrowserMainPartsWin : public BrowserMainParts {
 public:
  explicit BrowserMainPartsWin(const MainFunctionParams& parameters)
      : BrowserMainParts(parameters) {}

 protected:
  virtual void PreEarlyInitialization() {
    // Initialize Winsock.
    net::EnsureWinsockInit();
  }

  virtual void PreMainMessageLoopStart() {
    OleInitialize(NULL);
  }

 private:
  virtual void InitializeSSL() {
    // Use NSS for SSL by default.
    // Because of a build system issue (http://crbug.com/43461), the default
    // client socket factory uses SChannel (the system SSL library) for SSL by
    // default on Windows.
    if (!parsed_command_line().HasSwitch(switches::kUseSystemSSL)) {
      net::ClientSocketFactory::SetSSLClientSocketFactory(
          net::SSLClientSocketNSSFactory);
      // We want to be sure to init NSPR on the main thread.
      base::EnsureNSPRInit();
    }
  }
};

// static
BrowserMainParts* BrowserMainParts::CreateBrowserMainParts(
    const MainFunctionParams& parameters) {
  return new BrowserMainPartsWin(parameters);
}
<commit_msg>Sigh... in fixing the crash message for RTL, I broke LTR, due to AdjustStringForLocaleDirection() having stupid behavior.<commit_after>// Copyright (c) 2010 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 "chrome/browser/browser_main.h"
#include "chrome/browser/browser_main_win.h"

#include <windows.h>
#include <shellapi.h>

#include <algorithm>

#include "app/l10n_util.h"
#include "app/win_util.h"
#include "base/command_line.h"
#include "base/environment.h"
#include "base/i18n/rtl.h"
#include "base/nss_util.h"
#include "base/path_service.h"
#include "base/scoped_ptr.h"
#include "base/utf_string_conversions.h"
#include "base/win_util.h"
#include "chrome/browser/browser_list.h"
#include "chrome/browser/first_run/first_run.h"
#include "chrome/browser/metrics/metrics_service.h"
#include "chrome/browser/views/uninstall_view.h"
#include "chrome/common/chrome_switches.h"
#include "chrome/common/env_vars.h"
#include "chrome/common/result_codes.h"
#include "chrome/installer/util/helper.h"
#include "chrome/installer/util/install_util.h"
#include "chrome/installer/util/shell_util.h"
#include "grit/chromium_strings.h"
#include "grit/generated_resources.h"
#include "net/base/winsock_init.h"
#include "net/socket/ssl_client_socket_nss_factory.h"
#include "views/focus/accelerator_handler.h"
#include "views/window/window.h"

void DidEndMainMessageLoop() {
  OleUninitialize();
}

void RecordBreakpadStatusUMA(MetricsService* metrics) {
  DWORD len = ::GetEnvironmentVariableW(
      ASCIIToWide(env_vars::kNoOOBreakpad).c_str() , NULL, 0);
  metrics->RecordBreakpadRegistration((len == 0));
  metrics->RecordBreakpadHasDebugger(TRUE == ::IsDebuggerPresent());
}

void WarnAboutMinimumSystemRequirements() {
  if (win_util::GetWinVersion() < win_util::WINVERSION_XP) {
    // Display a warning message if the user is running chrome on Windows 2000.
    const std::wstring text =
        l10n_util::GetString(IDS_UNSUPPORTED_OS_PRE_WIN_XP);
    const std::wstring caption = l10n_util::GetString(IDS_PRODUCT_NAME);
    win_util::MessageBox(NULL, text, caption,
                         MB_OK | MB_ICONWARNING | MB_TOPMOST);
  }
}

int AskForUninstallConfirmation() {
  int ret = ResultCodes::NORMAL_EXIT;
  views::Window::CreateChromeWindow(NULL, gfx::Rect(),
                                    new UninstallView(ret))->Show();
  views::AcceleratorHandler accelerator_handler;
  MessageLoopForUI::current()->Run(&accelerator_handler);
  return ret;
}

void ShowCloseBrowserFirstMessageBox() {
  const std::wstring text = l10n_util::GetString(IDS_UNINSTALL_CLOSE_APP);
  const std::wstring caption = l10n_util::GetString(IDS_PRODUCT_NAME);
  const UINT flags = MB_OK | MB_ICONWARNING | MB_TOPMOST;
  win_util::MessageBox(NULL, text, caption, flags);
}

int DoUninstallTasks(bool chrome_still_running) {
  // We want to show a warning to user (and exit) if Chrome is already running
  // *before* we show the uninstall confirmation dialog box. But while the
  // uninstall confirmation dialog is up, user might start Chrome, so we
  // check once again after user acknowledges Uninstall dialog.
  if (chrome_still_running) {
    ShowCloseBrowserFirstMessageBox();
    return ResultCodes::UNINSTALL_CHROME_ALIVE;
  }
  int ret = AskForUninstallConfirmation();
  if (Upgrade::IsBrowserAlreadyRunning()) {
    ShowCloseBrowserFirstMessageBox();
    return ResultCodes::UNINSTALL_CHROME_ALIVE;
  }

  if (ret != ResultCodes::UNINSTALL_USER_CANCEL) {
    // The following actions are just best effort.
    LOG(INFO) << "Executing uninstall actions";
    if (!FirstRun::RemoveSentinel())
      LOG(INFO) << "Failed to delete sentinel file.";
    // We want to remove user level shortcuts and we only care about the ones
    // created by us and not by the installer so |alternate| is false.
    if (!ShellUtil::RemoveChromeDesktopShortcut(ShellUtil::CURRENT_USER, false))
      LOG(INFO) << "Failed to delete desktop shortcut.";
    if (!ShellUtil::RemoveChromeQuickLaunchShortcut(ShellUtil::CURRENT_USER))
      LOG(INFO) << "Failed to delete quick launch shortcut.";
  }
  return ret;
}

// Prepares the localized strings that are going to be displayed to
// the user if the browser process dies. These strings are stored in the
// environment block so they are accessible in the early stages of the
// chrome executable's lifetime.
void PrepareRestartOnCrashEnviroment(const CommandLine &parsed_command_line) {
  // Clear this var so child processes don't show the dialog by default.
  scoped_ptr<base::Environment> env(base::Environment::Create());
  env->UnSetVar(env_vars::kShowRestart);

  // For non-interactive tests we don't restart on crash.
  if (env->HasVar(env_vars::kHeadless))
    return;

  // If the known command-line test options are used we don't create the
  // environment block which means we don't get the restart dialog.
  if (parsed_command_line.HasSwitch(switches::kBrowserCrashTest) ||
      parsed_command_line.HasSwitch(switches::kBrowserAssertTest) ||
      parsed_command_line.HasSwitch(switches::kNoErrorDialogs))
    return;

  // The encoding we use for the info is "title|context|direction" where
  // direction is either env_vars::kRtlLocale or env_vars::kLtrLocale depending
  // on the current locale.
  string16 dlg_strings(l10n_util::GetStringUTF16(IDS_CRASH_RECOVERY_TITLE));
  dlg_strings.push_back('|');
  string16 adjusted_string(
      l10n_util::GetStringUTF16(IDS_CRASH_RECOVERY_CONTENT));
  base::i18n::AdjustStringForLocaleDirection(adjusted_string, &adjusted_string);
  dlg_strings.append(adjusted_string);
  dlg_strings.push_back('|');
  dlg_strings.append(ASCIIToUTF16(
      base::i18n::IsRTL() ? env_vars::kRtlLocale : env_vars::kLtrLocale));

  env->SetVar(env_vars::kRestartInfo, UTF16ToUTF8(dlg_strings));
}

// This method handles the --hide-icons and --show-icons command line options
// for chrome that get triggered by Windows from registry entries
// HideIconsCommand & ShowIconsCommand. Chrome doesn't support hide icons
// functionality so we just ask the users if they want to uninstall Chrome.
int HandleIconsCommands(const CommandLine &parsed_command_line) {
  if (parsed_command_line.HasSwitch(switches::kHideIcons)) {
    std::wstring cp_applet;
    win_util::WinVersion version = win_util::GetWinVersion();
    if (version >= win_util::WINVERSION_VISTA) {
      cp_applet.assign(L"Programs and Features");  // Windows Vista and later.
    } else if (version >= win_util::WINVERSION_XP) {
      cp_applet.assign(L"Add/Remove Programs");  // Windows XP.
    } else {
      return ResultCodes::UNSUPPORTED_PARAM;  // Not supported
    }

    const std::wstring msg = l10n_util::GetStringF(IDS_HIDE_ICONS_NOT_SUPPORTED,
                                                   cp_applet);
    const std::wstring caption = l10n_util::GetString(IDS_PRODUCT_NAME);
    const UINT flags = MB_OKCANCEL | MB_ICONWARNING | MB_TOPMOST;
    if (IDOK == win_util::MessageBox(NULL, msg, caption, flags))
      ShellExecute(NULL, NULL, L"appwiz.cpl", NULL, NULL, SW_SHOWNORMAL);
    return ResultCodes::NORMAL_EXIT;  // Exit as we are not launching browser.
  }
  // We don't hide icons so we shouldn't do anything special to show them
  return ResultCodes::UNSUPPORTED_PARAM;
}

// Check if there is any machine level Chrome installed on the current
// machine. If yes and the current Chrome process is user level, we do not
// allow the user level Chrome to run. So we notify the user and uninstall
// user level Chrome.
bool CheckMachineLevelInstall() {
  scoped_ptr<installer::Version> version(InstallUtil::GetChromeVersion(true));
  if (version.get()) {
    std::wstring exe;
    PathService::Get(base::DIR_EXE, &exe);
    std::transform(exe.begin(), exe.end(), exe.begin(), tolower);
    std::wstring user_exe_path = installer::GetChromeInstallPath(false);
    std::transform(user_exe_path.begin(), user_exe_path.end(),
                   user_exe_path.begin(), tolower);
    if (exe == user_exe_path) {
      const std::wstring text =
          l10n_util::GetString(IDS_MACHINE_LEVEL_INSTALL_CONFLICT);
      const std::wstring caption = l10n_util::GetString(IDS_PRODUCT_NAME);
      const UINT flags = MB_OK | MB_ICONERROR | MB_TOPMOST;
      win_util::MessageBox(NULL, text, caption, flags);
      std::wstring uninstall_cmd = InstallUtil::GetChromeUninstallCmd(false);
      if (!uninstall_cmd.empty()) {
        uninstall_cmd.append(L" --");
        uninstall_cmd.append(installer_util::switches::kForceUninstall);
        uninstall_cmd.append(L" --");
        uninstall_cmd.append(installer_util::switches::kDoNotRemoveSharedItems);
        base::LaunchApp(uninstall_cmd, false, false, NULL);
      }
      return true;
    }
  }
  return false;
}

// BrowserMainPartsWin ---------------------------------------------------------

class BrowserMainPartsWin : public BrowserMainParts {
 public:
  explicit BrowserMainPartsWin(const MainFunctionParams& parameters)
      : BrowserMainParts(parameters) {}

 protected:
  virtual void PreEarlyInitialization() {
    // Initialize Winsock.
    net::EnsureWinsockInit();
  }

  virtual void PreMainMessageLoopStart() {
    OleInitialize(NULL);
  }

 private:
  virtual void InitializeSSL() {
    // Use NSS for SSL by default.
    // Because of a build system issue (http://crbug.com/43461), the default
    // client socket factory uses SChannel (the system SSL library) for SSL by
    // default on Windows.
    if (!parsed_command_line().HasSwitch(switches::kUseSystemSSL)) {
      net::ClientSocketFactory::SetSSLClientSocketFactory(
          net::SSLClientSocketNSSFactory);
      // We want to be sure to init NSPR on the main thread.
      base::EnsureNSPRInit();
    }
  }
};

// static
BrowserMainParts* BrowserMainParts::CreateBrowserMainParts(
    const MainFunctionParams& parameters) {
  return new BrowserMainPartsWin(parameters);
}
<|endoftext|>


<commit_before>/* CritterDriver_debug.cpp
 * Seial driver for the main CritterBot hardware
 * James Neufeld (neufeld@cs.ualberta.ca)
 */

#include "CritterDriver_debug.h"
#include "CritterControlDrop.h"
#include "CritterStateDrop.h"
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>

CritterDriver_debug::CritterDriver_debug(DataLake *lake, ComponentConfig &conf,
                   string &name) : Component(lake, conf, name) {

  fid       = -1;
  controlId = lake->readyReading("CritterControlDrop");
  stateId   = lake->readyWriting("CritterStateDrop");
  acts      = 0;
  newData   = false;

  postWait = 10000;

  lastPost.setAsNow();
}

CritterDriver_debug::~CritterDriver_debug() {
  cleanup();
}

void CritterDriver_debug::cleanup() {

  lake->doneRead(controlId);
  lake->doneWriteHead(stateId);

}

void CritterDriver_debug::initport() {

}

void CritterDriver_debug::closeport() {

}

int CritterDriver_debug::init(USeconds &wokeAt) {

  printf("Opening FAKE serial port. weeee\n");

  printf("Serial port open with file descriptor %d.\n", fid);

  printf("Initializing FAKE serial port.\n");

  return 1;
}

void CritterDriver_debug::readPacket() {

  stateDrop.motor100.velocity = 10;
  stateDrop.motor100.current  = 10;
  stateDrop.motor100.temp     = 10;
  stateDrop.motor220.velocity = 20;
  stateDrop.motor220.current  = 20;
  stateDrop.motor220.temp     = 20;
  stateDrop.motor340.velocity = 30;
  stateDrop.motor340.current  = 30;
  stateDrop.motor340.temp     = 30;

  stateDrop.accel.x           = 10;
  stateDrop.accel.y           = 20;
  stateDrop.accel.z           = 30;
  stateDrop.mag.x             = 11;
  stateDrop.mag.y             = 21;
  stateDrop.mag.z             = 31;

  stateDrop.rotation          = 12;
  for(int j=0; j<10; j++) stateDrop.ir_distance[j] = j*10;
  for(int j=0; j<4; j++) stateDrop.light[j]        = j*11;

  stateDrop.error_flags = 0;
  for(int j=0; j<4; j++) {
    stateDrop.error_flags << 8;
    stateDrop.error_flags += j;
  }
  newData = true;

}

int CritterDriver_debug::think(USeconds &wokeAt) {
  sense(wokeAt);
  return 1;
}

int CritterDriver_debug::sense(USeconds &wokeAt) {

  readPacket();
  return 1;

}

int CritterDriver_debug::act(USeconds &now) {

  char str[100];

  if(now - lastPost >= postWait) {
    lastPost = now + (now - lastPost - postWait);
    acts++;

    controlDrop = (CritterControlDrop*)lake->readHead(controlId);

    if (controlDrop) {

      if (controlDrop->motor_mode == CritterControlDrop::WHEEL_SPACE) {

        sprintf(str,"motor 0 %3d\r",controlDrop->m100_vel);
        printf("CritterDriver_debug: FAKE wrote command: %s\n", str);

        sprintf(str,"motor 1 %3d\r",controlDrop->m220_vel);
        printf("CritterDriver_debug: FAKE wrote command: %s\n", str);

        sprintf(str,"motor 2 %3d\r",controlDrop->m340_vel);
        printf("CritterDriver_debug: FAKE wrote command: %s\n", str);

      } else { // assert(controlDrop->motor_mode == CritterControlDrop::XYTHETA_SPACE)

        int v1,v2,v3; // wheel velocities

        // [ v1 ]   [ cos(90+100) sin(90+100) 90 ]   [ v_x ]
        // [ v2 ] = [ cos(90+220) sin(90+220) 90 ] * [ v_y ]
        // [ v3 ]   [ cos(90+340) sin(90+340) 90 ]   [ v_theta ]
        v1 = (int)(-0.9848*controlDrop->x_vel + -0.1736*controlDrop->y_vel + 90*controlDrop->theta_vel);
        v2 = (int)(0.6428*controlDrop->x_vel + -0.766*controlDrop->y_vel + 90*controlDrop->theta_vel);
        v3 = (int)(0.342*controlDrop->x_vel + 0.9397*controlDrop->y_vel + 90*controlDrop->theta_vel);

        sprintf(str,"motor 0 %3d\r", v1);
        printf("CritterDriver_debug: FAKE wrote command: %s\n", str);

        sprintf(str,"motor 0 %3d\r", v2);
        printf("CritterDriver_debug: FAKE wrote command: %s\n", str);

        sprintf(str,"motor 0 %3d\r", v3);
        printf("CritterDriver_debug: FAKE wrote command: %s\n", str);

      }
    }
    lake->doneRead(controlId);

    if (newData) {
      (*(CritterStateDrop*)lake->startWriteHead(stateId)) = stateDrop;
      lake->doneWriteHead(stateId);
      newData = false;
    }
  }
  return 1;
}
<commit_msg>Renumbering the motors<commit_after>/* CritterDriver_debug.cpp
 * Seial driver for the main CritterBot hardware
 * James Neufeld (neufeld@cs.ualberta.ca)
 */

#include "CritterDriver_debug.h"
#include "CritterControlDrop.h"
#include "CritterStateDrop.h"
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>

CritterDriver_debug::CritterDriver_debug(DataLake *lake, ComponentConfig &conf,
                   string &name) : Component(lake, conf, name) {

  fid       = -1;
  controlId = lake->readyReading("CritterControlDrop");
  stateId   = lake->readyWriting("CritterStateDrop");
  acts      = 0;
  newData   = false;

  postWait = 10000;

  lastPost.setAsNow();
}

CritterDriver_debug::~CritterDriver_debug() {
  cleanup();
}

void CritterDriver_debug::cleanup() {

  lake->doneRead(controlId);
  lake->doneWriteHead(stateId);

}

void CritterDriver_debug::initport() {

}

void CritterDriver_debug::closeport() {

}

int CritterDriver_debug::init(USeconds &wokeAt) {

  printf("Opening FAKE serial port. weeee\n");

  printf("Serial port open with file descriptor %d.\n", fid);

  printf("Initializing FAKE serial port.\n");

  return 1;
}

void CritterDriver_debug::readPacket() {

  stateDrop.motor100.velocity = 10;
  stateDrop.motor100.current  = 10;
  stateDrop.motor100.temp     = 10;
  stateDrop.motor220.velocity = 20;
  stateDrop.motor220.current  = 20;
  stateDrop.motor220.temp     = 20;
  stateDrop.motor340.velocity = 30;
  stateDrop.motor340.current  = 30;
  stateDrop.motor340.temp     = 30;

  stateDrop.accel.x           = 10;
  stateDrop.accel.y           = 20;
  stateDrop.accel.z           = 30;
  stateDrop.mag.x             = 11;
  stateDrop.mag.y             = 21;
  stateDrop.mag.z             = 31;

  stateDrop.rotation          = 12;
  for(int j=0; j<10; j++) stateDrop.ir_distance[j] = j*10;
  for(int j=0; j<4; j++) stateDrop.light[j]        = j*11;

  stateDrop.error_flags = 0;
  for(int j=0; j<4; j++) {
    stateDrop.error_flags << 8;
    stateDrop.error_flags += j;
  }
  newData = true;

}

int CritterDriver_debug::think(USeconds &wokeAt) {
  sense(wokeAt);
  return 1;
}

int CritterDriver_debug::sense(USeconds &wokeAt) {

  readPacket();
  return 1;

}

int CritterDriver_debug::act(USeconds &now) {

  char str[100];

  if(now - lastPost >= postWait) {
    lastPost = now + (now - lastPost - postWait);
    acts++;

    controlDrop = (CritterControlDrop*)lake->readHead(controlId);

    if (controlDrop) {

      if (controlDrop->motor_mode == CritterControlDrop::WHEEL_SPACE) {

        sprintf(str,"motor 0 %3d\r",controlDrop->m100_vel);
        printf("CritterDriver_debug: FAKE wrote command: %s\n", str);

        sprintf(str,"motor 1 %3d\r",controlDrop->m220_vel);
        printf("CritterDriver_debug: FAKE wrote command: %s\n", str);

        sprintf(str,"motor 2 %3d\r",controlDrop->m340_vel);
        printf("CritterDriver_debug: FAKE wrote command: %s\n", str);

      } else { // assert(controlDrop->motor_mode == CritterControlDrop::XYTHETA_SPACE)

        int v1,v2,v3; // wheel velocities

        // [ v1 ]   [ cos(90+100) sin(90+100) 90 ]   [ v_x ]
        // [ v2 ] = [ cos(90+220) sin(90+220) 90 ] * [ v_y ]
        // [ v3 ]   [ cos(90+340) sin(90+340) 90 ]   [ v_theta ]
        v1 = (int)(-0.9848*controlDrop->x_vel + -0.1736*controlDrop->y_vel + 90*controlDrop->theta_vel);
        v2 = (int)(0.6428*controlDrop->x_vel + -0.766*controlDrop->y_vel + 90*controlDrop->theta_vel);
        v3 = (int)(0.342*controlDrop->x_vel + 0.9397*controlDrop->y_vel + 90*controlDrop->theta_vel);

        sprintf(str,"motor 0 %3d\r", v1);
        printf("CritterDriver_debug: FAKE wrote command: %s\n", str);

        sprintf(str,"motor 1 %3d\r", v2);
        printf("CritterDriver_debug: FAKE wrote command: %s\n", str);

        sprintf(str,"motor 2 %3d\r", v3);
        printf("CritterDriver_debug: FAKE wrote command: %s\n", str);

      }
    }
    lake->doneRead(controlId);

    if (newData) {
      (*(CritterStateDrop*)lake->startWriteHead(stateId)) = stateDrop;
      lake->doneWriteHead(stateId);
      newData = false;
    }
  }
  return 1;
}
<|endoftext|>


<commit_before>#include "JPetSigCh.h"

ClassImp(JPetSigCh);

void JPetSigCh::init(){
	fPM = NULL;
	fTRB = NULL;
	fScin = NULL;
	fBarrelSlot = NULL;
	fAmpl = 0;
	fIsSlow = 0;
}

JPetSigCh::JPetSigCh(const JPetSigCh& obj): TNamed("JPetSigCh", "Signal Channel Structure") {
	init();
	if (this != &obj){
		fAmpl = obj.getAmpl();
		fIsSlow = obj.isSlow();
		setPM(obj.getPM());
		setTRB(obj.getTRB());
		setScin(obj.getScin());
		setBarrelSlot(obj.getBarrelSlot());
		fChannels.clear();
		for (int i = 0; i < obj.getChSet().size(); i++) fChannels.push_back(obj.getChSet()[i]);

		//fChannels.obj.getChSet();
	}
}

JPetSigCh::JPetSigCh(float edge_time, float fall_edge_time): TNamed("JPetSigCh","Signal Channel Structure") {
	init();
	if (fall_edge_time == 0) fIsSlow = 1;
	addCh(edge_time, fall_edge_time);
}

template <class T>
void JPetSigCh::set(T** dest, const T& source) throw(bad_alloc){
	assert(dest != NULL);

	if ( &source == NULL && *dest != NULL ){
		delete *dest;
		*dest = NULL;
		return;
	}

	if (&source != NULL) {
		if (*dest == NULL){
			try { *dest = new T; }
			catch(bad_alloc& b_a){
				ERROR("Could not allocate memory.");
				ERROR(b_a.what());
			}
		}
		**dest = source;
	}
}

void JPetSigCh::addCh(float edge_time, float fall_edge_time){
	SingleCh tmp;
	tmp[kRising] = edge_time;
	tmp[kFalling] = fall_edge_time;
	fChannels.push_back(tmp);
}

JPetSigCh& JPetSigCh::operator=(const JPetSigCh& obj){
	JPetSigCh temp(obj);
  swap(temp, *this);
  return *this;
}
<commit_msg>drobna refaktoryzacja JPetSigCh<commit_after>#include "JPetSigCh.h"

ClassImp(JPetSigCh);

void JPetSigCh::init(){
	SetNameTitle("JPetSigCh", "Signal Channel Structure");
    fPM = NULL;
	fTRB = NULL;
	fScin = NULL;
	fBarrelSlot = NULL;
	fAmpl = 0;
	fIsSlow = 0;
}

JPetSigCh::JPetSigCh(const JPetSigCh& obj){
	init();
	if (this != &obj){
		fAmpl = obj.getAmpl();
		fIsSlow = obj.isSlow();
		setPM(obj.getPM());
		setTRB(obj.getTRB());
		setScin(obj.getScin());
		setBarrelSlot(obj.getBarrelSlot());
		fChannels.clear();
		for (int i = 0; i < obj.getChSet().size(); i++) fChannels.push_back(obj.getChSet()[i]);

		//fChannels.obj.getChSet();
	}
}

JPetSigCh::JPetSigCh(float edge_time, float fall_edge_time){
	init();
	if (fall_edge_time == 0) fIsSlow = 1;
	addCh(edge_time, fall_edge_time);
}

template <class T>
void JPetSigCh::set(T** dest, const T& source) throw(bad_alloc){
	assert(dest != NULL);

	if ( &source == NULL && *dest != NULL ){
		delete *dest;
		*dest = NULL;
		return;
	}

	if (&source != NULL) {
		if (*dest == NULL){
			try { *dest = new T; }
			catch(bad_alloc& b_a){
				ERROR("Could not allocate memory.");
				ERROR(b_a.what());
			}
		}
		**dest = source;
	}
}

void JPetSigCh::addCh(float edge_time, float fall_edge_time){
	SingleCh tmp;
	tmp[kRising] = edge_time;
	tmp[kFalling] = fall_edge_time;
	fChannels.push_back(tmp);
}

JPetSigCh& JPetSigCh::operator=(const JPetSigCh& obj){
	JPetSigCh temp(obj);
  swap(temp, *this);
  return *this;
}
<|endoftext|>


<commit_before>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#include "PythonMacros.h"
#include "PythonEnvironment.h"
#include "PythonScriptController.h"

#include <sofa/helper/system/FileRepository.h>
#include <sofa/helper/system/FileSystem.h>
#include <sofa/helper/system/SetDirectory.h>
#include <sofa/simulation/Node.h>

#include <sofa/helper/Utils.h>

#if defined(__linux__)
#  include <dlfcn.h>            // for dlopen(), see workaround in Init()
#endif

using namespace sofa::component::controller;

using sofa::helper::system::FileSystem;
using sofa::helper::Utils;

namespace sofa
{

namespace simulation
{

PyMODINIT_FUNC initModulesHelper(const std::string& name, PyMethodDef* methodDef)
{
    PythonEnvironment::gil lock(__func__);
    Py_InitModule(name.c_str(), methodDef);
}

void PythonEnvironment::addModule(const std::string& name, PyMethodDef* methodDef)
{
    initModulesHelper(name, methodDef);
}

void PythonEnvironment::Init()
{
    std::string pythonVersion = Py_GetVersion();

#ifndef NDEBUG
    SP_MESSAGE_INFO("Python version: " + pythonVersion)
#endif

#if defined(__linux__)
    // WARNING: workaround to be able to import python libraries on linux (like
    // numpy), at least on Ubuntu (see http://bugs.python.org/issue4434). It is
    // not fixing the real problem, but at least it is working for now.
    std::string pythonLibraryName = "libpython" + std::string(pythonVersion,0,3) + ".so";
    dlopen( pythonLibraryName.c_str(), RTLD_LAZY|RTLD_GLOBAL );
#endif

    // Prevent the python terminal from being buffered, not to miss or mix up traces.
    if( putenv( (char*)"PYTHONUNBUFFERED=1" ) )
        SP_MESSAGE_WARNING("failed to set environment variable PYTHONUNBUFFERED")

    if ( !Py_IsInitialized() )
    {
        Py_Initialize();
    }

    PyEval_InitThreads();

    // the first gil lock is here
    gil lock(__func__);

    // Append sofa modules to the embedded python environment.
    bindSofaPythonModule();

    // Required for sys.path, used in addPythonModulePath().
    PyRun_SimpleString("import sys");

    // Force C locale.
    PyRun_SimpleString("import locale");
    PyRun_SimpleString("locale.setlocale(locale.LC_ALL, 'C')");

    // Workaround: try to import numpy and to launch numpy.finfo to cache data;
    // this prevents a deadlock when calling numpy.finfo from a worker thread.
    // ocarre: may crash on some configurations, we have to find a fix
    PyRun_SimpleString("\
try:\n\
    import numpy\n\
    numpy.finfo(float)\n\
except:\n\
    pass");

    // If the script directory is not available (e.g. if the interpreter is invoked interactively
    // or if the script is read from standard input), path[0] is the empty string,
    // which directs Python to search modules in the current directory first.
    PyRun_SimpleString(std::string("sys.path.insert(0,\"\")").c_str());

    // Add the paths to the plugins' python modules to sys.path.  Those paths
    // are read from all the files in 'etc/sofa/python.d'
    std::string confDir = Utils::getSofaPathPrefix() + "/etc/sofa/python.d";
    if (FileSystem::exists(confDir))
    {
        std::vector<std::string> files;
        FileSystem::listDirectory(confDir, files);
        for (size_t i=0; i<files.size(); i++)
        {
            addPythonModulePathsFromConfigFile(confDir + "/" + files[i]);
        }
    }

    // Add the directories listed in the SOFAPYTHON_PLUGINS_PATH environnement
    // variable (colon-separated) to sys.path
    char * pathVar = getenv("SOFAPYTHON_PLUGINS_PATH");
    if (pathVar != NULL)
    {
        std::istringstream ss(pathVar);
        std::string path;
        while(std::getline(ss, path, ':'))
        {
            if (FileSystem::exists(path))
                addPythonModulePathsForPlugins(path);
            else
                SP_MESSAGE_WARNING("no such directory: '" + path + "'");
        }
    }

    // python livecoding related
    PyRun_SimpleString("from SofaPython.livecoding import onReimpAFile");

    // general sofa-python stuff
    PyRun_SimpleString("import SofaPython");

    // python modules are automatically reloaded at each scene loading
    setAutomaticModuleReload( true );
}

void PythonEnvironment::Release()
{
    // Finish the Python Interpreter

    // obviously can't use raii here
    PyGILState_Ensure();
    Py_Finalize();
}

void PythonEnvironment::addPythonModulePath(const std::string& path)
{
    static std::set<std::string> addedPath;
    if (addedPath.find(path)==addedPath.end()) {
        // note not to insert at first 0 place
        // an empty string must be at first so modules can be found in the current directory first.

        {
            gil lock(__func__);
            PyRun_SimpleString(std::string("sys.path.insert(1,\""+path+"\")").c_str());
        }

        SP_MESSAGE_INFO("Added '" + path + "' to sys.path");
        addedPath.insert(path);
    }
}

void PythonEnvironment::addPythonModulePathsFromConfigFile(const std::string& path)
{
    std::ifstream configFile(path.c_str());
    std::string line;
    while(std::getline(configFile, line))
    {
        if (!FileSystem::isAbsolute(line))
        {
            line = Utils::getSofaPathPrefix() + "/" + line;
        }
        addPythonModulePath(line);
    }
}

void PythonEnvironment::addPythonModulePathsForPlugins(const std::string& pluginsDirectory)
{
    std::vector<std::string> files;
    FileSystem::listDirectory(pluginsDirectory, files);

    for (std::vector<std::string>::iterator i = files.begin(); i != files.end(); ++i)
    {
        const std::string pluginPath = pluginsDirectory + "/" + *i;
        if (FileSystem::exists(pluginPath) && FileSystem::isDirectory(pluginPath))
        {
            const std::string pythonDir = pluginPath + "/python";
            if (FileSystem::exists(pythonDir) && FileSystem::isDirectory(pythonDir))
            {
                addPythonModulePath(pythonDir);
            }
        }
    }
}

/*
// helper functions
sofa::simulation::tree::GNode::SPtr PythonEnvironment::initGraphFromScript( const char *filename )
{
    PyObject *script = importScript(filename);
    if (!script)
        return 0;

    // the root node
    GNode::SPtr groot = sofa::core::objectmodel::New<GNode>(); // TODO: passer par une factory
    groot->setName( "root" );
   // groot->setGravity( Coord3(0,-10,0) );

    if (!initGraph(script,groot))
        groot = 0;

   else
        printf("Root node name after pyhton: %s\n",groot->getName().c_str());

    Py_DECREF(script);

    return groot;
}
*/

// some basic RAII stuff to handle init/termination cleanly
  namespace {

    struct raii {
      raii() {
          // initialization is done when loading the plugin
          // otherwise it can be executed too soon
          // when an application is directly linking with the SofaPython library
//        PythonEnvironment::Init();
      }

      ~raii() {
        PythonEnvironment::Release();
      }

    };

    static raii singleton;
  }

// basic script functions
std::string PythonEnvironment::getError()
{
    gil lock(__func__);
    std::string error;

    PyObject *ptype, *pvalue /* error msg */, *ptraceback /*stack snapshot and many other informations (see python traceback structure)*/;
    PyErr_Fetch(&ptype, &pvalue, &ptraceback);
    if(pvalue)
        error = PyString_AsString(pvalue);

    return error;
}

bool PythonEnvironment::runString(const std::string& script)
{
    gil lock(__func__);
    PyObject* pDict = PyModule_GetDict(PyImport_AddModule("__main__"));
    PyObject* result = PyRun_String(script.data(), Py_file_input, pDict, pDict);

    if(0 == result)
    {
        SP_MESSAGE_ERROR("Script (string) import error")
        PyErr_Print();

        return false;
    }

    Py_DECREF(result);

    return true;
}

std::string PythonEnvironment::getStackAsString()
{
    gil lock(__func__);
    PyObject* pDict = PyModule_GetDict(PyImport_AddModule("SofaPython"));
    PyObject* pFunc = PyDict_GetItemString(pDict, "getStackForSofa");
    if (PyCallable_Check(pFunc))
    {
        PyObject* res = PyObject_CallFunction(pFunc, nullptr);
        std::string tmp=PyString_AsString(PyObject_Str(res));
        Py_DECREF(res) ;
        return tmp;
    }
    return "Python Stack is empty.";
}

bool PythonEnvironment::runFile( const char *filename, const std::vector<std::string>& arguments)
{
    gil lock(__func__);

//    SP_MESSAGE_INFO( "Loading python script \""<<filename<<"\"" )
    std::string dir = sofa::helper::system::SetDirectory::GetParentDir(filename);
    std::string bareFilename = sofa::helper::system::SetDirectory::GetFileNameWithoutExtension(filename);
//    SP_MESSAGE_INFO( "script directory \""<<dir<<"\"" )

    //    SP_MESSAGE_INFO( commandString.c_str() )

    if(!arguments.empty())
    {
        char**argv = new char*[arguments.size()+1];
        argv[0] = new char[bareFilename.size()+1];
        strcpy( argv[0], bareFilename.c_str() );
        for( size_t i=0 ; i<arguments.size() ; ++i )
        {
            argv[i+1] = new char[arguments[i].size()+1];
            strcpy( argv[i+1], arguments[i].c_str() );
        }

        Py_SetProgramName(argv[0]); // TODO check what it is doing exactly

        PySys_SetArgv(arguments.size()+1, argv);

        for( size_t i=0 ; i<arguments.size()+1 ; ++i )
        {
            delete [] argv[i];
        }
        delete [] argv;
    }

    //  Py_BEGIN_ALLOW_THREADS

    // Load the scene script
    char* pythonFilename = strdup(filename);
    PyObject* scriptPyFile = PyFile_FromString(pythonFilename, (char*)("r"));
    free(pythonFilename);

    if( !scriptPyFile )
    {
        SP_MESSAGE_ERROR("cannot open file:" << filename)
        PyErr_Print();
        return false;
    }

    PyObject* pDict = PyModule_GetDict(PyImport_AddModule("__main__"));

    std::string backupFileName;
    PyObject* backupFileObject = PyDict_GetItemString(pDict, "__file__");
    if(backupFileObject)
        backupFileName = PyString_AsString(backupFileObject);

    PyObject* newFileObject = PyString_FromString(filename);
    PyDict_SetItemString(pDict, "__file__", newFileObject);

    int error = PyRun_SimpleFileEx(PyFile_AsFile(scriptPyFile), filename, 1);

    backupFileObject = PyString_FromString(backupFileName.c_str());
    PyDict_SetItemString(pDict, "__file__", backupFileObject);

    //  Py_END_ALLOW_THREADS

    if(0 != error)
    {
        SP_MESSAGE_ERROR("Script (file:" << bareFilename << ") import error")
        PyErr_Print();
        return false;
    }

    return true;
}

/*
bool PythonEnvironment::initGraph(PyObject *script, sofa::simulation::tree::GNode::SPtr graphRoot)  // calls the method "initGraph(root)" of the script
{
    // pDict is a borrowed reference
    PyObject *pDict = PyModule_GetDict(script);

    // pFunc is also a borrowed reference
    PyObject *pFunc = PyDict_GetItemString(pDict, "initGraph");

    if (PyCallable_Check(pFunc))
    {
      //  PyObject *args = PyTuple_New(1);
      //  PyTuple_SetItem(args,0,object(graphRoot.get()).ptr());

        try
        {
            //PyObject_CallObject(pFunc, NULL);//args);
            boost::python::call<int>(pFunc,boost::ref(*graphRoot.get()));
        }
        catch (const error_already_set e)
        {
            SP_MESSAGE_EXCEPTION("")
            PyErr_Print();

        }

      //  Py_DECREF(args);

        return true;
    }
    else
    {
        PyErr_Print();
        return false;
    }
}
*/

void PythonEnvironment::SceneLoaderListerner::rightBeforeLoadingScene()
{
    gil lock(__func__);
    // unload python modules to force importing their eventual modifications
    PyRun_SimpleString("SofaPython.unloadModules()");
}

void PythonEnvironment::setAutomaticModuleReload( bool b )
{
    if( b )
        SceneLoader::addListener( SceneLoaderListerner::getInstance() );
    else
        SceneLoader::removeListener( SceneLoaderListerner::getInstance() );
}

void PythonEnvironment::excludeModuleFromReload( const std::string& moduleName )
{
    gil lock(__func__);
    PyRun_SimpleString( std::string( "try: SofaPython.__SofaPythonEnvironment_modulesExcludedFromReload.append('" + moduleName + "')\nexcept:pass" ).c_str() );
}

static const bool debug_gil = false;

static PyGILState_STATE lock(const char* trace) {
    if(debug_gil && trace) {
        std::clog << ">> " << trace << " wants the gil" << std::endl;
    }

    // this ensures that we start with no active thread before first locking the
    // gil: this way the last gil unlock lets python threads to run (otherwise
    // the main thread still holds the gil, preventing python threads to run
    // until the main thread exits).

    // the first gil aquisition should happen right after the python interpreter
    // is initialized.
    static const PyThreadState* init = PyEval_SaveThread(); (void) init;

    return PyGILState_Ensure();
}

PythonEnvironment::gil::gil(const char* trace)
    : state(lock(trace)),
      trace(trace) { }

PythonEnvironment::gil::~gil() {

    PyGILState_Release(state);

    if(debug_gil && trace) {
        std::clog << "<< " << trace << " released the gil" << std::endl;
    }

}

PythonEnvironment::no_gil::no_gil(const char* trace)
    : state(PyEval_SaveThread()),
      trace(trace) {
    if(debug_gil && trace) {
        std::clog << ">> " << trace << " temporarily released the gil" << std::endl;
    }
}

PythonEnvironment::no_gil::~no_gil() {

    if(debug_gil && trace) {
        std::clog << "<< " << trace << " wants to reacquire the gil" << std::endl;
    }

    PyEval_RestoreThread(state);
}

} // namespace simulation

} // namespace sofa

<commit_msg>[SofaPython] fixed potential double finalization<commit_after>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#include "PythonMacros.h"
#include "PythonEnvironment.h"
#include "PythonScriptController.h"

#include <sofa/helper/system/FileRepository.h>
#include <sofa/helper/system/FileSystem.h>
#include <sofa/helper/system/SetDirectory.h>
#include <sofa/simulation/Node.h>

#include <sofa/helper/Utils.h>

#if defined(__linux__)
#  include <dlfcn.h>            // for dlopen(), see workaround in Init()
#endif

using namespace sofa::component::controller;

using sofa::helper::system::FileSystem;
using sofa::helper::Utils;

namespace sofa
{

namespace simulation
{

PyMODINIT_FUNC initModulesHelper(const std::string& name, PyMethodDef* methodDef)
{
    PythonEnvironment::gil lock(__func__);
    Py_InitModule(name.c_str(), methodDef);
}

void PythonEnvironment::addModule(const std::string& name, PyMethodDef* methodDef)
{
    initModulesHelper(name, methodDef);
}

void PythonEnvironment::Init()
{
    std::string pythonVersion = Py_GetVersion();

#ifndef NDEBUG
    SP_MESSAGE_INFO("Python version: " + pythonVersion)
#endif

#if defined(__linux__)
    // WARNING: workaround to be able to import python libraries on linux (like
    // numpy), at least on Ubuntu (see http://bugs.python.org/issue4434). It is
    // not fixing the real problem, but at least it is working for now.
    std::string pythonLibraryName = "libpython" + std::string(pythonVersion,0,3) + ".so";
    dlopen( pythonLibraryName.c_str(), RTLD_LAZY|RTLD_GLOBAL );
#endif

    // Prevent the python terminal from being buffered, not to miss or mix up traces.
    if( putenv( (char*)"PYTHONUNBUFFERED=1" ) )
        SP_MESSAGE_WARNING("failed to set environment variable PYTHONUNBUFFERED")

    if ( !Py_IsInitialized() )
    {
        Py_Initialize();
    }

    PyEval_InitThreads();

    // the first gil lock is here
    gil lock(__func__);

    // Append sofa modules to the embedded python environment.
    bindSofaPythonModule();

    // Required for sys.path, used in addPythonModulePath().
    PyRun_SimpleString("import sys");

    // Force C locale.
    PyRun_SimpleString("import locale");
    PyRun_SimpleString("locale.setlocale(locale.LC_ALL, 'C')");

    // Workaround: try to import numpy and to launch numpy.finfo to cache data;
    // this prevents a deadlock when calling numpy.finfo from a worker thread.
    // ocarre: may crash on some configurations, we have to find a fix
    PyRun_SimpleString("\
try:\n\
    import numpy\n\
    numpy.finfo(float)\n\
except:\n\
    pass");

    // If the script directory is not available (e.g. if the interpreter is invoked interactively
    // or if the script is read from standard input), path[0] is the empty string,
    // which directs Python to search modules in the current directory first.
    PyRun_SimpleString(std::string("sys.path.insert(0,\"\")").c_str());

    // Add the paths to the plugins' python modules to sys.path.  Those paths
    // are read from all the files in 'etc/sofa/python.d'
    std::string confDir = Utils::getSofaPathPrefix() + "/etc/sofa/python.d";
    if (FileSystem::exists(confDir))
    {
        std::vector<std::string> files;
        FileSystem::listDirectory(confDir, files);
        for (size_t i=0; i<files.size(); i++)
        {
            addPythonModulePathsFromConfigFile(confDir + "/" + files[i]);
        }
    }

    // Add the directories listed in the SOFAPYTHON_PLUGINS_PATH environnement
    // variable (colon-separated) to sys.path
    char * pathVar = getenv("SOFAPYTHON_PLUGINS_PATH");
    if (pathVar != NULL)
    {
        std::istringstream ss(pathVar);
        std::string path;
        while(std::getline(ss, path, ':'))
        {
            if (FileSystem::exists(path))
                addPythonModulePathsForPlugins(path);
            else
                SP_MESSAGE_WARNING("no such directory: '" + path + "'");
        }
    }

    // python livecoding related
    PyRun_SimpleString("from SofaPython.livecoding import onReimpAFile");

    // general sofa-python stuff
    PyRun_SimpleString("import SofaPython");

    // python modules are automatically reloaded at each scene loading
    setAutomaticModuleReload( true );
}

void PythonEnvironment::Release()
{
    // Finish the Python Interpreter

    // obviously can't use raii here
    if( Py_IsInitialized() ) {
        PyGILState_Ensure();
        Py_Finalize();
    }
}

void PythonEnvironment::addPythonModulePath(const std::string& path)
{
    static std::set<std::string> addedPath;
    if (addedPath.find(path)==addedPath.end()) {
        // note not to insert at first 0 place
        // an empty string must be at first so modules can be found in the current directory first.

        {
            gil lock(__func__);
            PyRun_SimpleString(std::string("sys.path.insert(1,\""+path+"\")").c_str());
        }

        SP_MESSAGE_INFO("Added '" + path + "' to sys.path");
        addedPath.insert(path);
    }
}

void PythonEnvironment::addPythonModulePathsFromConfigFile(const std::string& path)
{
    std::ifstream configFile(path.c_str());
    std::string line;
    while(std::getline(configFile, line))
    {
        if (!FileSystem::isAbsolute(line))
        {
            line = Utils::getSofaPathPrefix() + "/" + line;
        }
        addPythonModulePath(line);
    }
}

void PythonEnvironment::addPythonModulePathsForPlugins(const std::string& pluginsDirectory)
{
    std::vector<std::string> files;
    FileSystem::listDirectory(pluginsDirectory, files);

    for (std::vector<std::string>::iterator i = files.begin(); i != files.end(); ++i)
    {
        const std::string pluginPath = pluginsDirectory + "/" + *i;
        if (FileSystem::exists(pluginPath) && FileSystem::isDirectory(pluginPath))
        {
            const std::string pythonDir = pluginPath + "/python";
            if (FileSystem::exists(pythonDir) && FileSystem::isDirectory(pythonDir))
            {
                addPythonModulePath(pythonDir);
            }
        }
    }
}

/*
// helper functions
sofa::simulation::tree::GNode::SPtr PythonEnvironment::initGraphFromScript( const char *filename )
{
    PyObject *script = importScript(filename);
    if (!script)
        return 0;

    // the root node
    GNode::SPtr groot = sofa::core::objectmodel::New<GNode>(); // TODO: passer par une factory
    groot->setName( "root" );
   // groot->setGravity( Coord3(0,-10,0) );

    if (!initGraph(script,groot))
        groot = 0;

   else
        printf("Root node name after pyhton: %s\n",groot->getName().c_str());

    Py_DECREF(script);

    return groot;
}
*/

// some basic RAII stuff to handle init/termination cleanly
  namespace {

    struct raii {
      raii() {
          // initialization is done when loading the plugin
          // otherwise it can be executed too soon
          // when an application is directly linking with the SofaPython library
//        PythonEnvironment::Init();
      }

      ~raii() {
        PythonEnvironment::Release();
      }

    };

    static raii singleton;
  }

// basic script functions
std::string PythonEnvironment::getError()
{
    gil lock(__func__);
    std::string error;

    PyObject *ptype, *pvalue /* error msg */, *ptraceback /*stack snapshot and many other informations (see python traceback structure)*/;
    PyErr_Fetch(&ptype, &pvalue, &ptraceback);
    if(pvalue)
        error = PyString_AsString(pvalue);

    return error;
}

bool PythonEnvironment::runString(const std::string& script)
{
    gil lock(__func__);
    PyObject* pDict = PyModule_GetDict(PyImport_AddModule("__main__"));
    PyObject* result = PyRun_String(script.data(), Py_file_input, pDict, pDict);

    if(0 == result)
    {
        SP_MESSAGE_ERROR("Script (string) import error")
        PyErr_Print();

        return false;
    }

    Py_DECREF(result);

    return true;
}

std::string PythonEnvironment::getStackAsString()
{
    gil lock(__func__);
    PyObject* pDict = PyModule_GetDict(PyImport_AddModule("SofaPython"));
    PyObject* pFunc = PyDict_GetItemString(pDict, "getStackForSofa");
    if (PyCallable_Check(pFunc))
    {
        PyObject* res = PyObject_CallFunction(pFunc, nullptr);
        std::string tmp=PyString_AsString(PyObject_Str(res));
        Py_DECREF(res) ;
        return tmp;
    }
    return "Python Stack is empty.";
}

bool PythonEnvironment::runFile( const char *filename, const std::vector<std::string>& arguments)
{
    gil lock(__func__);

//    SP_MESSAGE_INFO( "Loading python script \""<<filename<<"\"" )
    std::string dir = sofa::helper::system::SetDirectory::GetParentDir(filename);
    std::string bareFilename = sofa::helper::system::SetDirectory::GetFileNameWithoutExtension(filename);
//    SP_MESSAGE_INFO( "script directory \""<<dir<<"\"" )

    //    SP_MESSAGE_INFO( commandString.c_str() )

    if(!arguments.empty())
    {
        char**argv = new char*[arguments.size()+1];
        argv[0] = new char[bareFilename.size()+1];
        strcpy( argv[0], bareFilename.c_str() );
        for( size_t i=0 ; i<arguments.size() ; ++i )
        {
            argv[i+1] = new char[arguments[i].size()+1];
            strcpy( argv[i+1], arguments[i].c_str() );
        }

        Py_SetProgramName(argv[0]); // TODO check what it is doing exactly

        PySys_SetArgv(arguments.size()+1, argv);

        for( size_t i=0 ; i<arguments.size()+1 ; ++i )
        {
            delete [] argv[i];
        }
        delete [] argv;
    }

    //  Py_BEGIN_ALLOW_THREADS

    // Load the scene script
    char* pythonFilename = strdup(filename);
    PyObject* scriptPyFile = PyFile_FromString(pythonFilename, (char*)("r"));
    free(pythonFilename);

    if( !scriptPyFile )
    {
        SP_MESSAGE_ERROR("cannot open file:" << filename)
        PyErr_Print();
        return false;
    }

    PyObject* pDict = PyModule_GetDict(PyImport_AddModule("__main__"));

    std::string backupFileName;
    PyObject* backupFileObject = PyDict_GetItemString(pDict, "__file__");
    if(backupFileObject)
        backupFileName = PyString_AsString(backupFileObject);

    PyObject* newFileObject = PyString_FromString(filename);
    PyDict_SetItemString(pDict, "__file__", newFileObject);

    int error = PyRun_SimpleFileEx(PyFile_AsFile(scriptPyFile), filename, 1);

    backupFileObject = PyString_FromString(backupFileName.c_str());
    PyDict_SetItemString(pDict, "__file__", backupFileObject);

    //  Py_END_ALLOW_THREADS

    if(0 != error)
    {
        SP_MESSAGE_ERROR("Script (file:" << bareFilename << ") import error")
        PyErr_Print();
        return false;
    }

    return true;
}

/*
bool PythonEnvironment::initGraph(PyObject *script, sofa::simulation::tree::GNode::SPtr graphRoot)  // calls the method "initGraph(root)" of the script
{
    // pDict is a borrowed reference
    PyObject *pDict = PyModule_GetDict(script);

    // pFunc is also a borrowed reference
    PyObject *pFunc = PyDict_GetItemString(pDict, "initGraph");

    if (PyCallable_Check(pFunc))
    {
      //  PyObject *args = PyTuple_New(1);
      //  PyTuple_SetItem(args,0,object(graphRoot.get()).ptr());

        try
        {
            //PyObject_CallObject(pFunc, NULL);//args);
            boost::python::call<int>(pFunc,boost::ref(*graphRoot.get()));
        }
        catch (const error_already_set e)
        {
            SP_MESSAGE_EXCEPTION("")
            PyErr_Print();

        }

      //  Py_DECREF(args);

        return true;
    }
    else
    {
        PyErr_Print();
        return false;
    }
}
*/

void PythonEnvironment::SceneLoaderListerner::rightBeforeLoadingScene()
{
    gil lock(__func__);
    // unload python modules to force importing their eventual modifications
    PyRun_SimpleString("SofaPython.unloadModules()");
}

void PythonEnvironment::setAutomaticModuleReload( bool b )
{
    if( b )
        SceneLoader::addListener( SceneLoaderListerner::getInstance() );
    else
        SceneLoader::removeListener( SceneLoaderListerner::getInstance() );
}

void PythonEnvironment::excludeModuleFromReload( const std::string& moduleName )
{
    gil lock(__func__);
    PyRun_SimpleString( std::string( "try: SofaPython.__SofaPythonEnvironment_modulesExcludedFromReload.append('" + moduleName + "')\nexcept:pass" ).c_str() );
}

static const bool debug_gil = false;

static PyGILState_STATE lock(const char* trace) {
    if(debug_gil && trace) {
        std::clog << ">> " << trace << " wants the gil" << std::endl;
    }

    // this ensures that we start with no active thread before first locking the
    // gil: this way the last gil unlock lets python threads to run (otherwise
    // the main thread still holds the gil, preventing python threads to run
    // until the main thread exits).

    // the first gil aquisition should happen right after the python interpreter
    // is initialized.
    static const PyThreadState* init = PyEval_SaveThread(); (void) init;

    return PyGILState_Ensure();
}

PythonEnvironment::gil::gil(const char* trace)
    : state(lock(trace)),
      trace(trace) { }

PythonEnvironment::gil::~gil() {

    PyGILState_Release(state);

    if(debug_gil && trace) {
        std::clog << "<< " << trace << " released the gil" << std::endl;
    }

}

PythonEnvironment::no_gil::no_gil(const char* trace)
    : state(PyEval_SaveThread()),
      trace(trace) {
    if(debug_gil && trace) {
        std::clog << ">> " << trace << " temporarily released the gil" << std::endl;
    }
}

PythonEnvironment::no_gil::~no_gil() {

    if(debug_gil && trace) {
        std::clog << "<< " << trace << " wants to reacquire the gil" << std::endl;
    }

    PyEval_RestoreThread(state);
}

} // namespace simulation

} // namespace sofa

<|endoftext|>


<commit_before>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4      *
*                (c) 2006-2009 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU General Public License as published by the Free  *
* Software Foundation; either version 2 of the License, or (at your option)   *
* any later version.                                                          *
*                                                                             *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for    *
* more details.                                                               *
*                                                                             *
* You should have received a copy of the GNU General Public License along     *
* with this program; if not, write to the Free Software Foundation, Inc., 51  *
* Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.                   *
*******************************************************************************
*                            SOFA :: Applications                             *
*                                                                             *
* Authors: M. Adam, J. Allard, B. Andre, P-J. Bensoussan, S. Cotin, C. Duriez,*
* H. Delingette, F. Falipou, F. Faure, S. Fonteneau, L. Heigeas, C. Mendoza,  *
* M. Nesme, P. Neumann, J-P. de la Plata Alcade, F. Poyer and F. Roy          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#include "GenerateRigid.h"

#include <stdlib.h>
#include <math.h>
#include <sofa/helper/vector.h>

namespace projects
{

bool GenerateRigid(sofa::defaulttype::Rigid3Mass& mass, sofa::defaulttype::Vec3d& center, sofa::helper::io::Mesh* mesh)
{
    using namespace sofa::defaulttype;
    using namespace sofa::helper;
    // Geometric Tools, Inc.
    // http://www.geometrictools.com
    // Copyright (c) 1998-2006.	All Rights Reserved
    //
    // The Wild Magic Library (WM3) source code is supplied under the terms of
    // the license agreement
    //     http://www.geometrictools.com/License/WildMagic3License.pdf
    // and may not be copied or disclosed except in accordance with the terms
    // of that agreement.

    // order:	1, x, y, z, x^2, y^2, z^2, xy, yz, zx
    double afIntegral[10] = { (double)0.0, (double)0.0, (double)0.0, (double)0.0,
            (double)0.0, (double)0.0, (double)0.0, (double)0.0, (double)0.0, (double)0.0
                            };

    const vector<Vector3>& points = mesh->getVertices();
    const vector< vector< vector<int> > >& facets = mesh->getFacets();
    for (unsigned int i = 0; i < facets.size(); i++)
    {
        const vector<int>& v = facets[i][0];
        for (unsigned int j = 2; j < v.size(); j++)
        {
            // get vertices of current triangle
            const Vector3 kV0 = points[v[0  ]];
            const Vector3 kV1 = points[v[j-1]];
            const Vector3 kV2 = points[v[j  ]];

            // get cross product of edges
            Vector3 kV1mV0 = kV1 - kV0;
            Vector3 kV2mV0 = kV2 - kV0;
            Vector3 kN = cross(kV1mV0,kV2mV0);

            // compute integral terms
            double fTmp0, fTmp1, fTmp2;
            double fF1x, fF2x, fF3x, fG0x, fG1x, fG2x;
            fTmp0 = kV0[0] + kV1[0];
            fF1x = fTmp0 + kV2[0];
            fTmp1 = kV0[0]*kV0[0];
            fTmp2 = fTmp1 + kV1[0]*fTmp0;
            fF2x = fTmp2 + kV2[0]*fF1x;
            fF3x = kV0[0]*fTmp1 + kV1[0]*fTmp2 + kV2[0]*fF2x;
            fG0x = fF2x + kV0[0]*(fF1x + kV0[0]);
            fG1x = fF2x + kV1[0]*(fF1x + kV1[0]);
            fG2x = fF2x + kV2[0]*(fF1x + kV2[0]);

            double fF1y, fF2y, fF3y, fG0y, fG1y, fG2y;
            fTmp0 = kV0[1] + kV1[1];
            fF1y = fTmp0 + kV2[1];
            fTmp1 = kV0[1]*kV0[1];
            fTmp2 = fTmp1 + kV1[1]*fTmp0;
            fF2y = fTmp2 + kV2[1]*fF1y;
            fF3y = kV0[1]*fTmp1 + kV1[1]*fTmp2 + kV2[1]*fF2y;
            fG0y = fF2y + kV0[1]*(fF1y + kV0[1]);
            fG1y = fF2y + kV1[1]*(fF1y + kV1[1]);
            fG2y = fF2y + kV2[1]*(fF1y + kV2[1]);

            double fF1z, fF2z, fF3z, fG0z, fG1z, fG2z;
            fTmp0 = kV0[2] + kV1[2];
            fF1z = fTmp0 + kV2[2];
            fTmp1 = kV0[2]*kV0[2];
            fTmp2 = fTmp1 + kV1[2]*fTmp0;
            fF2z = fTmp2 + kV2[2]*fF1z;
            fF3z = kV0[2]*fTmp1 + kV1[2]*fTmp2 + kV2[2]*fF2z;
            fG0z = fF2z + kV0[2]*(fF1z + kV0[2]);
            fG1z = fF2z + kV1[2]*(fF1z + kV1[2]);
            fG2z = fF2z + kV2[2]*(fF1z + kV2[2]);

            // update integrals
            afIntegral[0] += kN[0]*fF1x;
            afIntegral[1] += kN[0]*fF2x;
            afIntegral[2] += kN[1]*fF2y;
            afIntegral[3] += kN[2]*fF2z;
            afIntegral[4] += kN[0]*fF3x;
            afIntegral[5] += kN[1]*fF3y;
            afIntegral[6] += kN[2]*fF3z;
            afIntegral[7] += kN[0]*(kV0[1]*fG0x + kV1[1]*fG1x + kV2[1]*fG2x);
            afIntegral[8] += kN[1]*(kV0[2]*fG0y + kV1[2]*fG1y + kV2[2]*fG2y);
            afIntegral[9] += kN[2]*(kV0[0]*fG0z + kV1[0]*fG1z + kV2[0]*fG2z);
        }
    }

    afIntegral[0] /= 6.0;
    afIntegral[1] /= 24.0;
    afIntegral[2] /= 24.0;
    afIntegral[3] /= 24.0;
    afIntegral[4] /= 60.0;
    afIntegral[5] /= 60.0;
    afIntegral[6] /= 60.0;
    afIntegral[7] /= 120.0;
    afIntegral[8] /= 120.0;
    afIntegral[9] /= 120.0;

    // mass
    mass.mass = mass.volume = afIntegral[0];

    // center of mass
    center = Vector3(afIntegral[1]/afIntegral[0],afIntegral[2]/afIntegral[0],afIntegral[3]/afIntegral[0]);

    // inertia relative to world origin
    mass.inertiaMatrix[0][0] = afIntegral[5] + afIntegral[6];
    mass.inertiaMatrix[0][1] = -afIntegral[7];
    mass.inertiaMatrix[0][2] = -afIntegral[9];
    mass.inertiaMatrix[1][0] = mass.inertiaMatrix[0][1];
    mass.inertiaMatrix[1][1] = afIntegral[4] + afIntegral[6];
    mass.inertiaMatrix[1][2] = -afIntegral[8];
    mass.inertiaMatrix[2][0] = mass.inertiaMatrix[0][2];
    mass.inertiaMatrix[2][1] = mass.inertiaMatrix[1][2];
    mass.inertiaMatrix[2][2] = afIntegral[4] + afIntegral[5];

    // inertia relative to center of mass
    mass.inertiaMatrix[0][0] -= mass.mass*(center[1]*center[1] + center[2]*center[2]);
    mass.inertiaMatrix[0][1] += mass.mass*center[0]*center[1];
    mass.inertiaMatrix[0][2] += mass.mass*center[2]*center[0];
    mass.inertiaMatrix[1][0] = mass.inertiaMatrix[0][1];
    mass.inertiaMatrix[1][1] -= mass.mass*(center[2]*center[2] + center[0]*center[0]);
    mass.inertiaMatrix[1][2] += mass.mass*center[1]*center[2];
    mass.inertiaMatrix[2][0] = mass.inertiaMatrix[0][2];
    mass.inertiaMatrix[2][1] = mass.inertiaMatrix[1][2];
    mass.inertiaMatrix[2][2] -= mass.mass*(center[0]*center[0] + center[1]*center[1]);

    mass.inertiaMatrix /= mass.mass;

    mass.recalc();
    return true;
}

}
<commit_msg>r5247/sofa-dev : FIX: problem with metrics: mass should be volume*1000<commit_after>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4      *
*                (c) 2006-2009 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU General Public License as published by the Free  *
* Software Foundation; either version 2 of the License, or (at your option)   *
* any later version.                                                          *
*                                                                             *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for    *
* more details.                                                               *
*                                                                             *
* You should have received a copy of the GNU General Public License along     *
* with this program; if not, write to the Free Software Foundation, Inc., 51  *
* Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.                   *
*******************************************************************************
*                            SOFA :: Applications                             *
*                                                                             *
* Authors: M. Adam, J. Allard, B. Andre, P-J. Bensoussan, S. Cotin, C. Duriez,*
* H. Delingette, F. Falipou, F. Faure, S. Fonteneau, L. Heigeas, C. Mendoza,  *
* M. Nesme, P. Neumann, J-P. de la Plata Alcade, F. Poyer and F. Roy          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#include "GenerateRigid.h"

#include <stdlib.h>
#include <math.h>
#include <sofa/helper/vector.h>

namespace projects
{

bool GenerateRigid(sofa::defaulttype::Rigid3Mass& mass, sofa::defaulttype::Vec3d& center, sofa::helper::io::Mesh* mesh)
{
    using namespace sofa::defaulttype;
    using namespace sofa::helper;
    // Geometric Tools, Inc.
    // http://www.geometrictools.com
    // Copyright (c) 1998-2006.	All Rights Reserved
    //
    // The Wild Magic Library (WM3) source code is supplied under the terms of
    // the license agreement
    //     http://www.geometrictools.com/License/WildMagic3License.pdf
    // and may not be copied or disclosed except in accordance with the terms
    // of that agreement.

    // order:	1, x, y, z, x^2, y^2, z^2, xy, yz, zx
    double afIntegral[10] = { (double)0.0, (double)0.0, (double)0.0, (double)0.0,
            (double)0.0, (double)0.0, (double)0.0, (double)0.0, (double)0.0, (double)0.0
                            };

    const vector<Vector3>& points = mesh->getVertices();
    const vector< vector< vector<int> > >& facets = mesh->getFacets();
    for (unsigned int i = 0; i < facets.size(); i++)
    {
        const vector<int>& v = facets[i][0];
        for (unsigned int j = 2; j < v.size(); j++)
        {
            // get vertices of current triangle
            const Vector3 kV0 = points[v[0  ]];
            const Vector3 kV1 = points[v[j-1]];
            const Vector3 kV2 = points[v[j  ]];

            // get cross product of edges
            Vector3 kV1mV0 = kV1 - kV0;
            Vector3 kV2mV0 = kV2 - kV0;
            Vector3 kN = cross(kV1mV0,kV2mV0);

            // compute integral terms
            double fTmp0, fTmp1, fTmp2;
            double fF1x, fF2x, fF3x, fG0x, fG1x, fG2x;
            fTmp0 = kV0[0] + kV1[0];
            fF1x = fTmp0 + kV2[0];
            fTmp1 = kV0[0]*kV0[0];
            fTmp2 = fTmp1 + kV1[0]*fTmp0;
            fF2x = fTmp2 + kV2[0]*fF1x;
            fF3x = kV0[0]*fTmp1 + kV1[0]*fTmp2 + kV2[0]*fF2x;
            fG0x = fF2x + kV0[0]*(fF1x + kV0[0]);
            fG1x = fF2x + kV1[0]*(fF1x + kV1[0]);
            fG2x = fF2x + kV2[0]*(fF1x + kV2[0]);

            double fF1y, fF2y, fF3y, fG0y, fG1y, fG2y;
            fTmp0 = kV0[1] + kV1[1];
            fF1y = fTmp0 + kV2[1];
            fTmp1 = kV0[1]*kV0[1];
            fTmp2 = fTmp1 + kV1[1]*fTmp0;
            fF2y = fTmp2 + kV2[1]*fF1y;
            fF3y = kV0[1]*fTmp1 + kV1[1]*fTmp2 + kV2[1]*fF2y;
            fG0y = fF2y + kV0[1]*(fF1y + kV0[1]);
            fG1y = fF2y + kV1[1]*(fF1y + kV1[1]);
            fG2y = fF2y + kV2[1]*(fF1y + kV2[1]);

            double fF1z, fF2z, fF3z, fG0z, fG1z, fG2z;
            fTmp0 = kV0[2] + kV1[2];
            fF1z = fTmp0 + kV2[2];
            fTmp1 = kV0[2]*kV0[2];
            fTmp2 = fTmp1 + kV1[2]*fTmp0;
            fF2z = fTmp2 + kV2[2]*fF1z;
            fF3z = kV0[2]*fTmp1 + kV1[2]*fTmp2 + kV2[2]*fF2z;
            fG0z = fF2z + kV0[2]*(fF1z + kV0[2]);
            fG1z = fF2z + kV1[2]*(fF1z + kV1[2]);
            fG2z = fF2z + kV2[2]*(fF1z + kV2[2]);

            // update integrals
            afIntegral[0] += kN[0]*fF1x;
            afIntegral[1] += kN[0]*fF2x;
            afIntegral[2] += kN[1]*fF2y;
            afIntegral[3] += kN[2]*fF2z;
            afIntegral[4] += kN[0]*fF3x;
            afIntegral[5] += kN[1]*fF3y;
            afIntegral[6] += kN[2]*fF3z;
            afIntegral[7] += kN[0]*(kV0[1]*fG0x + kV1[1]*fG1x + kV2[1]*fG2x);
            afIntegral[8] += kN[1]*(kV0[2]*fG0y + kV1[2]*fG1y + kV2[2]*fG2y);
            afIntegral[9] += kN[2]*(kV0[0]*fG0z + kV1[0]*fG1z + kV2[0]*fG2z);
        }
    }

    afIntegral[0] /= 6.0;
    afIntegral[1] /= 24.0;
    afIntegral[2] /= 24.0;
    afIntegral[3] /= 24.0;
    afIntegral[4] /= 60.0;
    afIntegral[5] /= 60.0;
    afIntegral[6] /= 60.0;
    afIntegral[7] /= 120.0;
    afIntegral[8] /= 120.0;
    afIntegral[9] /= 120.0;

    // mass
    mass.volume = afIntegral[0];
    mass.mass = 1000 * mass.volume; // using standard metrics, 1 cubic meter of a material with density 1 weights 1000 kg

    // center of mass
    center = Vector3(afIntegral[1]/afIntegral[0],afIntegral[2]/afIntegral[0],afIntegral[3]/afIntegral[0]);

    // inertia relative to world origin
    mass.inertiaMatrix[0][0] = afIntegral[5] + afIntegral[6];
    mass.inertiaMatrix[0][1] = -afIntegral[7];
    mass.inertiaMatrix[0][2] = -afIntegral[9];
    mass.inertiaMatrix[1][0] = mass.inertiaMatrix[0][1];
    mass.inertiaMatrix[1][1] = afIntegral[4] + afIntegral[6];
    mass.inertiaMatrix[1][2] = -afIntegral[8];
    mass.inertiaMatrix[2][0] = mass.inertiaMatrix[0][2];
    mass.inertiaMatrix[2][1] = mass.inertiaMatrix[1][2];
    mass.inertiaMatrix[2][2] = afIntegral[4] + afIntegral[5];

    // inertia relative to center of mass
    mass.inertiaMatrix[0][0] -= mass.mass*(center[1]*center[1] + center[2]*center[2]);
    mass.inertiaMatrix[0][1] += mass.mass*center[0]*center[1];
    mass.inertiaMatrix[0][2] += mass.mass*center[2]*center[0];
    mass.inertiaMatrix[1][0] = mass.inertiaMatrix[0][1];
    mass.inertiaMatrix[1][1] -= mass.mass*(center[2]*center[2] + center[0]*center[0]);
    mass.inertiaMatrix[1][2] += mass.mass*center[1]*center[2];
    mass.inertiaMatrix[2][0] = mass.inertiaMatrix[0][2];
    mass.inertiaMatrix[2][1] = mass.inertiaMatrix[1][2];
    mass.inertiaMatrix[2][2] -= mass.mass*(center[0]*center[0] + center[1]*center[1]);

    mass.inertiaMatrix /= mass.mass;

    mass.recalc();
    return true;
}

}
<|endoftext|>


<commit_before>#include "SBL.h"
#include <math/random.h>
using namespace std;
typedef SBLPlanner::Node Node;

SBLPlanner::SBLPlanner(CSpace* s)
  :space(s),maxExtendDistance(0.2),maxExtendIters(10),edgeConnectionThreshold(Inf),numIters(0),tStart(NULL),tGoal(NULL)
{}

SBLPlanner::~SBLPlanner()
{
  Cleanup();
}

void SBLPlanner::Cleanup()
{
  SafeDelete(tStart);
  SafeDelete(tGoal);
  outputPath.clear();
  numIters=0;
}

void SBLPlanner::Init(const Config& qStart,const Config& qGoal)
{
  Assert(!tStart && !tGoal);
  tStart = new SBLTreeWithIndex(space);
  tGoal = new SBLTreeWithIndex(space);
  tStart->Init(qStart);
  tGoal->Init(qGoal);
  //cout<<"SBL: Distance "<<space->Distance(qStart,qGoal)<<endl;
  //cout<<"SBL: Distance "<<space->Distance(*tStart,*tGoal)<<endl;
  if(CheckPath(tStart->root,tGoal->root)) {
    cout<<"SBLPlanner::Init(): Start and goal connected!"<<endl;
  }
}

bool SBLPlanner::Extend()
{
  numIters++;
  int useStart = RandBool();
  SBLTree *s, *g;
  if(useStart) { s=tStart; g=tGoal; }
  else { s=tGoal; g=tStart; }
  Node* ns=s->Extend(maxExtendDistance,maxExtendIters);
  if(ns) {
    Node* ng=PickConnection(g,*ns);
    if(s == tStart) return CheckPath(ns,ng);
    else return CheckPath(ng,ns);
  }
  return false;
}

bool SBLPlanner::CheckPath(Node* nStart,Node* nGoal)
{
  Assert(outputPath.empty());
  if(!IsInf(edgeConnectionThreshold)) {
    if(space->Distance(*nStart,*nGoal) > edgeConnectionThreshold) return false;
  }
  return SBLTree::CheckPath(tStart,nStart,tGoal,nGoal,outputPath);
}

void CreateMilestonePath(const list<SBLTree::EdgeInfo>& in,MilestonePath& out)
{
  Assert(!in.empty());
  out.edges.resize(in.size());
  int index=0;
  for(list<SBLTree::EdgeInfo>::const_iterator i=in.begin();i!=in.end();i++) {
    if(i->reversed) out.edges[index] = i->e->ReverseCopy();
    else out.edges[index] = i->e->Copy();
    Assert(out.edges[index]->Start() == *i->s);
    Assert(out.edges[index]->Goal() == *i->t);
    index++;
  }
}

void SBLPlanner::CreatePath(MilestonePath& path) const
{
  CreateMilestonePath(outputPath,path);
  Assert(path.edges.front()->Start() == *tStart->root);
  Assert(path.edges.back()->Goal() == *tGoal->root);
}

SBLPlannerWithGrid::SBLPlannerWithGrid(CSpace* s)
  :SBLPlanner(s),numItersPerRandomize(50),gridDivision(0.1)
{}

void SBLPlannerWithGrid::Init(const Config& qStart,const Config& qGoal)
{
  SBLTreeWithGrid* s= new SBLTreeWithGrid(space);
  SBLTreeWithGrid* g= new SBLTreeWithGrid(space);
  tStart = s;
  tGoal = g;
  s->Init(qStart);
  g->Init(qGoal);

  s->A.h.resize(qStart.n,gridDivision);
  g->A.h.resize(qStart.n,gridDivision);

  if(CheckPath(s->root,g->root)) {
    cout<<"SBLPlanner::Init(): Start and goal connected!"<<endl;
  }
}

Node* SBLPlannerWithGrid::PickConnection(SBLTree* t,const Config& x)
{
  SBLTreeWithGrid* s=(SBLTreeWithGrid*)t;
  return s->FindNearby(x);
}

void SBLPlannerWithGrid::Cleanup()
{
  SBLPlanner::Cleanup();
}

bool SBLPlannerWithGrid::Extend()
{
  if((numIters+1) % numItersPerRandomize == 0) RandomizeSubset();

  return SBLPlanner::Extend();
}

void SBLPlannerWithGrid::RandomizeSubset()
{
  SBLTreeWithGrid* s=(SBLTreeWithGrid*)tStart;
  SBLTreeWithGrid* g=(SBLTreeWithGrid*)tGoal;
  s->RandomizeSubset();
  g->RandomizeSubset();
}

SBLPRT::SBLPRT(CSpace* s)
  :space(s),maxExtendDistance(0.2),maxExtendIters(10),
   defaultPPickClosestTree(0.2),defaultPPickClosestNode(0.0),
   numIters(0)
{}

SBLPRT::~SBLPRT()
{
  Cleanup();
}

void SBLPRT::Cleanup()
{
  for(size_t i=0;i<roadmap.nodes.size();i++)
    delete roadmap.nodes[i];
  roadmap.Cleanup();
  numIters=0;
}

int SBLPRT::AddSeed(const Config& q)
{
  //SBLTree* t = new SBLTreeWithIndex(space);
  SBLTreeWithGrid* t = new SBLTreeWithGrid(space);
  t->A.h.resize(q.n,0.1);
  t->RandomizeSubset();
  ccs.AddNode();
  t->Init(q);
  Assert(space->IsFeasible(q));
  return roadmap.AddNode(t);
}

pair<int,int> SBLPRT::Expand()
{
  numIters++;
  if(numIters % 50 == 0) {
    for(size_t i=0;i<roadmap.nodes.size();i++)
      ((SBLTreeWithGrid*)roadmap.nodes[i])->RandomizeSubset();
  }
  int t=RandInt(roadmap.nodes.size());
  if(!IsSeedFullyConnected(t)) {
    int g=ExpandTree(t);
    if(g >= 0)  return pair<int,int>(t,g);
  }
  return pair<int,int>(-1,-1);
}

int SBLPRT::ExpandTree(int t)
{
  //printf("SBLPRT: Extend\n");
  Node* n=roadmap.nodes[t]->Extend(maxExtendDistance,maxExtendIters);
  if(!n) {
    //printf("SBLPRT: No extend...\n");
    return -1;
  }
  //printf("SBLPRT: PickConnection\n");
  pair<int,Node*> con=PickConnection(t,n);
  int tg = con.first;
  Node* ng = con.second;
  if(tg < 0 && ng == NULL) { cerr<<"Warning, picked a nonexistent connection"<<endl; return -1; }

  MilestonePath* p=roadmap.FindEdge(t,tg);
  Assert(p != NULL);
  Assert(p->edges.empty());

  list<EdgeInfo> outputPath;
  if(SBLTree::CheckPath(roadmap.nodes[t],n,roadmap.nodes[tg],ng,outputPath)) {
    //cout<<"Connecting nodes "<<t<<" to "<<tg<<endl;
    CreateMilestonePath(outputPath,*p);
    ccs.AddEdge(t,tg);
    return tg;
  }
  //printf("SBLPRT: No connect...\n");
  return -1;
}

void SBLPRT::AddRoadmapEdgesIfBelowThreshold(Real distanceThreshold)
{
  int n=roadmap.NumNodes();
  for(int i=0;i<n;i++)
    for(int j=i+1;i<n;i++)
      if(space->Distance(*roadmap.nodes[i]->root,*roadmap.nodes[j]->root) < distanceThreshold)
	AddRoadmapEdge(i,j);

}

bool SBLPRT::IsEdgeConnected(int i,int j) const
{
  const MilestonePath*e=roadmap.FindEdge(i,j);
  if(!e) return false;
  //Assert(e!=NULL);
  return !e->edges.empty();
}

bool SBLPRT::IsSeedFullyConnected(int i) const
{
  Roadmap::Iterator e;
  for(roadmap.Begin(i,e);!e.end();e++) {
    if(e->edges.empty()) {
      if(ccs.GetComponent(e.source()) != ccs.GetComponent(e.target()))
	return false;
    }
  }
  return true;
}

struct ConnectedSeedCallback : public Graph::PathIntCallback
{
  SBLPRT* prt;

  ConnectedSeedCallback(SBLPRT* _prt,int seeknode) :Graph::PathIntCallback(_prt->roadmap.NumNodes(),seeknode),prt(_prt) {}
  virtual bool ForwardEdge(int i,int j)
  {
    MilestonePath* p=prt->roadmap.FindEdge(i,j);
    Assert(p!=NULL);
    if(p->edges.empty()) return false;
    Assert(parents[j]==-1);
    parents[j]=i;
    return true;
  }
};

/*
bool SBLPRT::AreSeedsConnected(int i,int j)
{
  ConnectedSeedCallback callback(this,j);
  callback.node = j;
  roadmap.NewTraversal();
  roadmap._DFS(i,callback);
  if(callback.found) return true;
  return false;
}
*/

struct PathInfo
{
  int tstart,tgoal;
  MilestonePath* path;
};

void SBLPRT::CreatePath(int i,int j,MilestonePath& path)
{
  Assert(i >= 0 && i < (int)roadmap.nodes.size());
  Assert(j >= 0 && j < (int)roadmap.nodes.size());
  ConnectedSeedCallback callback(this,j);
  callback.node = j;
  roadmap.NewTraversal();
  roadmap._BFS(i,callback);
  if(!callback.found) {
    cerr<<"SBLPRT::CreatePath: Warning, a path doesn't exist between nodes "<<i<<" and "<<j<<endl;
    return;
  }

  list<PathInfo> subpaths;
  int n=j;
  while(n != i) {
    int p=callback.parents[n];
    Assert(p >= 0);
    PathInfo temp;
    temp.tstart = p;
    temp.tgoal = n;
    temp.path = roadmap.FindEdge(n,p);
    Assert(temp.path != NULL);
    Assert(!temp.path->edges.empty());
    subpaths.push_front(temp);
    n=p;
  }
  for(list<PathInfo>::iterator it=subpaths.begin();it!=subpaths.end();it++) {
    int s=it->tstart;
    int g=it->tgoal;
    Assert(s >= 0 && s <(int)roadmap.nodes.size());
    Assert(g >= 0 && g <(int)roadmap.nodes.size());
    MilestonePath& subpath=*it->path;
    Assert(!subpath.edges.empty());
    //forward path or backward path?
    if(subpath.edges.front()->Start() == *roadmap.nodes[s]->root) {
      Assert(subpath.edges.back()->Goal() == *roadmap.nodes[g]->root);
      //forward path
      path.Concat(subpath);
      if(!path.IsValid()) fprintf(stderr,"SBLPRT::CreatePath: Path invalidated on %d %d\n",s,g);
    }
    else {
      Assert(subpath.edges.front()->Start() == *roadmap.nodes[g]->root);
      Assert(subpath.edges.back()->Goal() == *roadmap.nodes[s]->root);
      //backward path
      for(int k=(int)subpath.edges.size()-1;k>=0;k--) {
	path.edges.push_back(subpath.edges[k]->ReverseCopy());
      }
      if(!path.IsValid()) fprintf(stderr,"SBLPRT::CreatePath: Path invalidated on backwards %d %d\n",s,g);
    }
  }
  Assert(path.IsValid());
}

pair<int,Node*> SBLPRT::PickConnection(int t,Node* n)
{
  pair<int,Node*> res(-1,(Node*)NULL);
  if(RandBool(defaultPPickClosestTree)) res.first=PickClosestAdjacentTree(t,*n);
  else res.first=PickRandomAdjacentTree(t);
  if(res.first >= 0) {
    if(RandBool(defaultPPickClosestNode)) res.second=GetClosestNode(res.first,*n);
    else res.second=PickNode(res.first);
  }
  return res;
}

int SBLPRT::PickRandomAdjacentTree(int t)
{
  vector<int> adj;
  Roadmap::Iterator e;
  for(roadmap.Begin(t,e);!e.end();e++) {
    if(e->edges.empty() && (ccs.GetComponent(t) != ccs.GetComponent(e.target()))) adj.push_back(e.target());
  }
  if(adj.empty()) return -1;
  return adj[RandInt(adj.size())];
}

int SBLPRT::PickClosestAdjacentTree(int t,const Config& x)
{
  int closest=-1;
  Real closestDist=Inf;
  Roadmap::Iterator e;
  for(roadmap.Begin(t,e);!e.end();e++) {
    if(e->edges.empty() && (ccs.GetComponent(t) != ccs.GetComponent(e.target()))) {
      const Config& y=*roadmap.nodes[e.target()]->root;
      Real d=space->Distance(x,y);
      if(d < closestDist) {
	closest = e.target();
	closestDist = d;
      }
    }
  }
  return closest;
}

<commit_msg>Fixed SBL for < 3d problems<commit_after>#include "SBL.h"
#include <math/random.h>
using namespace std;
typedef SBLPlanner::Node Node;

SBLPlanner::SBLPlanner(CSpace* s)
  :space(s),maxExtendDistance(0.2),maxExtendIters(10),edgeConnectionThreshold(Inf),numIters(0),tStart(NULL),tGoal(NULL)
{}

SBLPlanner::~SBLPlanner()
{
  Cleanup();
}

void SBLPlanner::Cleanup()
{
  SafeDelete(tStart);
  SafeDelete(tGoal);
  outputPath.clear();
  numIters=0;
}

void SBLPlanner::Init(const Config& qStart,const Config& qGoal)
{
  Assert(!tStart && !tGoal);
  tStart = new SBLTreeWithIndex(space);
  tGoal = new SBLTreeWithIndex(space);
  tStart->Init(qStart);
  tGoal->Init(qGoal);
  //cout<<"SBL: Distance "<<space->Distance(qStart,qGoal)<<endl;
  //cout<<"SBL: Distance "<<space->Distance(*tStart,*tGoal)<<endl;
  if(CheckPath(tStart->root,tGoal->root)) {
    cout<<"SBLPlanner::Init(): Start and goal connected!"<<endl;
  }
}

bool SBLPlanner::Extend()
{
  numIters++;
  int useStart = RandBool();
  SBLTree *s, *g;
  if(useStart) { s=tStart; g=tGoal; }
  else { s=tGoal; g=tStart; }
  Node* ns=s->Extend(maxExtendDistance,maxExtendIters);
  if(ns) {
    Node* ng=PickConnection(g,*ns);
    if(s == tStart) return CheckPath(ns,ng);
    else return CheckPath(ng,ns);
  }
  return false;
}

bool SBLPlanner::CheckPath(Node* nStart,Node* nGoal)
{
  Assert(outputPath.empty());
  if(!IsInf(edgeConnectionThreshold)) {
    if(space->Distance(*nStart,*nGoal) > edgeConnectionThreshold) return false;
  }
  return SBLTree::CheckPath(tStart,nStart,tGoal,nGoal,outputPath);
}

void CreateMilestonePath(const list<SBLTree::EdgeInfo>& in,MilestonePath& out)
{
  Assert(!in.empty());
  out.edges.resize(in.size());
  int index=0;
  for(list<SBLTree::EdgeInfo>::const_iterator i=in.begin();i!=in.end();i++) {
    if(i->reversed) out.edges[index] = i->e->ReverseCopy();
    else out.edges[index] = i->e->Copy();
    Assert(out.edges[index]->Start() == *i->s);
    Assert(out.edges[index]->Goal() == *i->t);
    index++;
  }
}

void SBLPlanner::CreatePath(MilestonePath& path) const
{
  CreateMilestonePath(outputPath,path);
  Assert(path.edges.front()->Start() == *tStart->root);
  Assert(path.edges.back()->Goal() == *tGoal->root);
}

SBLPlannerWithGrid::SBLPlannerWithGrid(CSpace* s)
  :SBLPlanner(s),numItersPerRandomize(50),gridDivision(0.1)
{}

void SBLPlannerWithGrid::Init(const Config& qStart,const Config& qGoal)
{
  SBLTreeWithGrid* s= new SBLTreeWithGrid(space);
  SBLTreeWithGrid* g= new SBLTreeWithGrid(space);
  tStart = s;
  tGoal = g;
  s->Init(qStart);
  g->Init(qGoal);

  s->A.h.resize(qStart.n,gridDivision);
  g->A.h.resize(qStart.n,gridDivision);
  //this is kinda stupid, should figure out a cleaner way to do this
  if(qStart.n < (int)s->A.subsetToFull.mapping.size()) {
    s->A.subsetToFull.mapping.resize(qStart.n);
    g->A.subsetToFull.mapping.resize(qStart.n);
    s->A.temp.resize(qStart.n);
    g->A.temp.resize(qStart.n);
    s->A.subdiv.h.n = qStart.n;
    g->A.subdiv.h.n = qStart.n;
  }
  if(CheckPath(s->root,g->root)) {
    cout<<"SBLPlanner::Init(): Start and goal connected!"<<endl;
  }
}

Node* SBLPlannerWithGrid::PickConnection(SBLTree* t,const Config& x)
{
  SBLTreeWithGrid* s=(SBLTreeWithGrid*)t;
  return s->FindNearby(x);
}

void SBLPlannerWithGrid::Cleanup()
{
  SBLPlanner::Cleanup();
}

bool SBLPlannerWithGrid::Extend()
{
  if((numIters+1) % numItersPerRandomize == 0) RandomizeSubset();

  return SBLPlanner::Extend();
}

void SBLPlannerWithGrid::RandomizeSubset()
{
  SBLTreeWithGrid* s=(SBLTreeWithGrid*)tStart;
  SBLTreeWithGrid* g=(SBLTreeWithGrid*)tGoal;
  s->RandomizeSubset();
  g->RandomizeSubset();
}

SBLPRT::SBLPRT(CSpace* s)
  :space(s),maxExtendDistance(0.2),maxExtendIters(10),
   defaultPPickClosestTree(0.2),defaultPPickClosestNode(0.0),
   numIters(0)
{}

SBLPRT::~SBLPRT()
{
  Cleanup();
}

void SBLPRT::Cleanup()
{
  for(size_t i=0;i<roadmap.nodes.size();i++)
    delete roadmap.nodes[i];
  roadmap.Cleanup();
  numIters=0;
}

int SBLPRT::AddSeed(const Config& q)
{
  //SBLTree* t = new SBLTreeWithIndex(space);
  SBLTreeWithGrid* t = new SBLTreeWithGrid(space);
  t->A.h.resize(q.n,0.1);
  t->RandomizeSubset();
  ccs.AddNode();
  t->Init(q);
  Assert(space->IsFeasible(q));
  return roadmap.AddNode(t);
}

pair<int,int> SBLPRT::Expand()
{
  numIters++;
  if(numIters % 50 == 0) {
    for(size_t i=0;i<roadmap.nodes.size();i++)
      ((SBLTreeWithGrid*)roadmap.nodes[i])->RandomizeSubset();
  }
  int t=RandInt(roadmap.nodes.size());
  if(!IsSeedFullyConnected(t)) {
    int g=ExpandTree(t);
    if(g >= 0)  return pair<int,int>(t,g);
  }
  return pair<int,int>(-1,-1);
}

int SBLPRT::ExpandTree(int t)
{
  //printf("SBLPRT: Extend\n");
  Node* n=roadmap.nodes[t]->Extend(maxExtendDistance,maxExtendIters);
  if(!n) {
    //printf("SBLPRT: No extend...\n");
    return -1;
  }
  //printf("SBLPRT: PickConnection\n");
  pair<int,Node*> con=PickConnection(t,n);
  int tg = con.first;
  Node* ng = con.second;
  if(tg < 0 && ng == NULL) { cerr<<"Warning, picked a nonexistent connection"<<endl; return -1; }

  MilestonePath* p=roadmap.FindEdge(t,tg);
  Assert(p != NULL);
  Assert(p->edges.empty());

  list<EdgeInfo> outputPath;
  if(SBLTree::CheckPath(roadmap.nodes[t],n,roadmap.nodes[tg],ng,outputPath)) {
    //cout<<"Connecting nodes "<<t<<" to "<<tg<<endl;
    CreateMilestonePath(outputPath,*p);
    ccs.AddEdge(t,tg);
    return tg;
  }
  //printf("SBLPRT: No connect...\n");
  return -1;
}

void SBLPRT::AddRoadmapEdgesIfBelowThreshold(Real distanceThreshold)
{
  int n=roadmap.NumNodes();
  for(int i=0;i<n;i++)
    for(int j=i+1;i<n;i++)
      if(space->Distance(*roadmap.nodes[i]->root,*roadmap.nodes[j]->root) < distanceThreshold)
	AddRoadmapEdge(i,j);

}

bool SBLPRT::IsEdgeConnected(int i,int j) const
{
  const MilestonePath*e=roadmap.FindEdge(i,j);
  if(!e) return false;
  //Assert(e!=NULL);
  return !e->edges.empty();
}

bool SBLPRT::IsSeedFullyConnected(int i) const
{
  Roadmap::Iterator e;
  for(roadmap.Begin(i,e);!e.end();e++) {
    if(e->edges.empty()) {
      if(ccs.GetComponent(e.source()) != ccs.GetComponent(e.target()))
	return false;
    }
  }
  return true;
}

struct ConnectedSeedCallback : public Graph::PathIntCallback
{
  SBLPRT* prt;

  ConnectedSeedCallback(SBLPRT* _prt,int seeknode) :Graph::PathIntCallback(_prt->roadmap.NumNodes(),seeknode),prt(_prt) {}
  virtual bool ForwardEdge(int i,int j)
  {
    MilestonePath* p=prt->roadmap.FindEdge(i,j);
    Assert(p!=NULL);
    if(p->edges.empty()) return false;
    Assert(parents[j]==-1);
    parents[j]=i;
    return true;
  }
};

/*
bool SBLPRT::AreSeedsConnected(int i,int j)
{
  ConnectedSeedCallback callback(this,j);
  callback.node = j;
  roadmap.NewTraversal();
  roadmap._DFS(i,callback);
  if(callback.found) return true;
  return false;
}
*/

struct PathInfo
{
  int tstart,tgoal;
  MilestonePath* path;
};

void SBLPRT::CreatePath(int i,int j,MilestonePath& path)
{
  Assert(i >= 0 && i < (int)roadmap.nodes.size());
  Assert(j >= 0 && j < (int)roadmap.nodes.size());
  ConnectedSeedCallback callback(this,j);
  callback.node = j;
  roadmap.NewTraversal();
  roadmap._BFS(i,callback);
  if(!callback.found) {
    cerr<<"SBLPRT::CreatePath: Warning, a path doesn't exist between nodes "<<i<<" and "<<j<<endl;
    return;
  }

  list<PathInfo> subpaths;
  int n=j;
  while(n != i) {
    int p=callback.parents[n];
    Assert(p >= 0);
    PathInfo temp;
    temp.tstart = p;
    temp.tgoal = n;
    temp.path = roadmap.FindEdge(n,p);
    Assert(temp.path != NULL);
    Assert(!temp.path->edges.empty());
    subpaths.push_front(temp);
    n=p;
  }
  for(list<PathInfo>::iterator it=subpaths.begin();it!=subpaths.end();it++) {
    int s=it->tstart;
    int g=it->tgoal;
    Assert(s >= 0 && s <(int)roadmap.nodes.size());
    Assert(g >= 0 && g <(int)roadmap.nodes.size());
    MilestonePath& subpath=*it->path;
    Assert(!subpath.edges.empty());
    //forward path or backward path?
    if(subpath.edges.front()->Start() == *roadmap.nodes[s]->root) {
      Assert(subpath.edges.back()->Goal() == *roadmap.nodes[g]->root);
      //forward path
      path.Concat(subpath);
      if(!path.IsValid()) fprintf(stderr,"SBLPRT::CreatePath: Path invalidated on %d %d\n",s,g);
    }
    else {
      Assert(subpath.edges.front()->Start() == *roadmap.nodes[g]->root);
      Assert(subpath.edges.back()->Goal() == *roadmap.nodes[s]->root);
      //backward path
      for(int k=(int)subpath.edges.size()-1;k>=0;k--) {
	path.edges.push_back(subpath.edges[k]->ReverseCopy());
      }
      if(!path.IsValid()) fprintf(stderr,"SBLPRT::CreatePath: Path invalidated on backwards %d %d\n",s,g);
    }
  }
  Assert(path.IsValid());
}

pair<int,Node*> SBLPRT::PickConnection(int t,Node* n)
{
  pair<int,Node*> res(-1,(Node*)NULL);
  if(RandBool(defaultPPickClosestTree)) res.first=PickClosestAdjacentTree(t,*n);
  else res.first=PickRandomAdjacentTree(t);
  if(res.first >= 0) {
    if(RandBool(defaultPPickClosestNode)) res.second=GetClosestNode(res.first,*n);
    else res.second=PickNode(res.first);
  }
  return res;
}

int SBLPRT::PickRandomAdjacentTree(int t)
{
  vector<int> adj;
  Roadmap::Iterator e;
  for(roadmap.Begin(t,e);!e.end();e++) {
    if(e->edges.empty() && (ccs.GetComponent(t) != ccs.GetComponent(e.target()))) adj.push_back(e.target());
  }
  if(adj.empty()) return -1;
  return adj[RandInt(adj.size())];
}

int SBLPRT::PickClosestAdjacentTree(int t,const Config& x)
{
  int closest=-1;
  Real closestDist=Inf;
  Roadmap::Iterator e;
  for(roadmap.Begin(t,e);!e.end();e++) {
    if(e->edges.empty() && (ccs.GetComponent(t) != ccs.GetComponent(e.target()))) {
      const Config& y=*roadmap.nodes[e.target()]->root;
      Real d=space->Distance(x,y);
      if(d < closestDist) {
	closest = e.target();
	closestDist = d;
      }
    }
  }
  return closest;
}

<|endoftext|>