<commit_before>/*
 * Copyright (c) 2011, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * Author: Mario Prats
 */

#include <ros/ros.h>
#include <tf/transform_datatypes.h>

#include <boost/thread.hpp>
#include <boost/program_options/variables_map.hpp>

#include <moveit/warehouse/planning_scene_storage.h>
#include <moveit/warehouse/constraints_storage.h>
#include <moveit/warehouse/planning_scene_world_storage.h>
#include <moveit/kinematics_constraint_aware/kinematics_constraint_aware.h>
#include <moveit/kinematics_cache/kinematics_cache.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/kinematic_state/kinematic_state.h>
#include <moveit/benchmarks/benchmarks_config.h>

int main(int argc, char** argv)
{
  boost::program_options::options_description desc;
  desc.add_options()
          ("help", "Show help message")
          ("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
          ("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.");

  boost::program_options::variables_map vm;
  boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
  boost::program_options::notify(vm);

  if (vm.count("help"))
  {
    std::cout << desc << std::endl;
    return 1;
  }

  ros::init(argc, argv, "arm_workspace_tests");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  moveit_benchmarks::BenchmarkConfig bc(vm.count("host") ? vm["host"].as<std::string>() : "",
      vm.count("port") ? vm["port"].as<std::size_t>() : 0);

  std::string db_host = vm["host"].as<std::string>();
  int db_port = vm["port"].as<std::size_t>();

  planning_models_loader::KinematicModelLoader kinematic_model_loader("robot_description"); /** Used to load the robot model */
  kinematic_model::KinematicModelPtr kinematic_model = kinematic_model_loader.getModel();

  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor;
  planning_scene_monitor.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description"));

  unsigned int proc = 0;
  for (int i = 1 ; i < argc ; ++i)
  {
    if (bc.readOptions(argv[i]))
    {
      std::stringstream ss;
      bc.printOptions(ss);
      ROS_INFO("Calling kinematic reachability benchmark with options:\n%s\n", ss.str().c_str());
      proc++;

      std::string group_name, frame_id, scene_name;

      scene_name = bc.getOptions().scene;
      group_name = bc.getOptions().group_override;
      frame_id = bc.getOptions().planning_frame;

      moveit_warehouse::PlanningSceneStorage pss(db_host, db_port);
      moveit_warehouse::PlanningSceneWithMetadata pswm;
      pss.getPlanningScene(pswm, scene_name);
      moveit_msgs::PlanningScene ps = static_cast<const moveit_msgs::PlanningScene&>(*pswm);
      planning_scene_monitor->getPlanningScene()->usePlanningSceneMsg(ps);

      ROS_INFO_STREAM("Processing scene " << planning_scene_monitor->getPlanningScene()->getName());

      kinematics_constraint_aware::KinematicsConstraintAwarePtr kinematics_constraint_aware;
      kinematics_constraint_aware.reset(new kinematics_constraint_aware::KinematicsConstraintAware(kinematic_model, group_name));

      moveit_warehouse::ConstraintsStorage cs(db_host, db_port);
      std::vector<std::string> constraints_names;
      cs.getKnownConstraints(bc.getOptions().goal_regex, constraints_names);
      for (int i=0; i<constraints_names.size(); i++)
      {
        moveit_warehouse::ConstraintsWithMetadata cwm;
        cs.getConstraints(cwm, constraints_names[i]);

        geometry_msgs::PoseStamped ik_pose;
        ik_pose.header.frame_id = frame_id;
        ik_pose.pose.position.x = cwm->position_constraints[0].constraint_region.primitive_poses[0].position.x;
        ik_pose.pose.position.y = cwm->position_constraints[0].constraint_region.primitive_poses[0].position.y;
        ik_pose.pose.position.z = cwm->position_constraints[0].constraint_region.primitive_poses[0].position.z;
        ik_pose.pose.orientation.x = cwm->orientation_constraints[0].orientation.x;
        ik_pose.pose.orientation.y = cwm->orientation_constraints[0].orientation.y;
        ik_pose.pose.orientation.z = cwm->orientation_constraints[0].orientation.z;
        ik_pose.pose.orientation.w = cwm->orientation_constraints[0].orientation.w;

        moveit_msgs::MoveItErrorCodes error_code;
        moveit_msgs::RobotState solution;

        //Fill the IK Request msg
        moveit_msgs::GetConstraintAwarePositionIK::Request request;
        moveit_msgs::GetConstraintAwarePositionIK::Response response;

        kinematic_model::KinematicModelConstPtr kinematic_model = kinematics_constraint_aware->getKinematicModel();
        kinematic_state::KinematicState kinematic_state(kinematic_model);
        const kinematic_model::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup(group_name);
        kinematic_state::JointStateGroup* joint_state_group = kinematic_state.getJointStateGroup(group_name);
        joint_state_group->setToRandomValues();

        request.timeout = ros::Duration(5.0);
        request.ik_request.robot_state.joint_state.name = joint_model_group->getJointModelNames();
        joint_state_group->getVariableValues(request.ik_request.robot_state.joint_state.position);
        request.ik_request.group_name = group_name;
        request.ik_request.pose_stamped = ik_pose;

        //Compute IK
        kinematics_constraint_aware->getIK(planning_scene_monitor->getPlanningScene(),request,response);

        if (response.error_code.val == response.error_code.SUCCESS)
        {
          ROS_INFO_STREAM(" Goal " << constraints_names[i] << " SUCCESS!");
        }
        else
        {
          ROS_INFO_STREAM(" Goal " << constraints_names[i] << " FAIL with code " << (int)response.error_code.val);
        }
      }
    }
  }
  ROS_INFO("Processed %u benchmark configuration files", proc);

  return(0);
}
<commit_msg>Output to file<commit_after>/*
 * Copyright (c) 2011, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * Author: Mario Prats
 */

#include <ros/ros.h>
#include <tf/transform_datatypes.h>

#include <boost/thread.hpp>
#include <boost/program_options/variables_map.hpp>

#include <moveit/warehouse/planning_scene_storage.h>
#include <moveit/warehouse/constraints_storage.h>
#include <moveit/warehouse/planning_scene_world_storage.h>
#include <moveit/kinematics_constraint_aware/kinematics_constraint_aware.h>
#include <moveit/kinematics_cache/kinematics_cache.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/kinematic_state/kinematic_state.h>
#include <moveit/benchmarks/benchmarks_config.h>

#include <fstream>

int main(int argc, char** argv)
{
  boost::program_options::options_description desc;
  desc.add_options()
          ("help", "Show help message")
          ("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
          ("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.");

  boost::program_options::variables_map vm;
  boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
  boost::program_options::notify(vm);

  if (vm.count("help") || ! vm.count("host") || ! vm.count("port"))
  {
    std::cout << desc << std::endl;
    return 1;
  }

  ros::init(argc, argv, "arm_workspace_tests");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  moveit_benchmarks::BenchmarkConfig bc(vm.count("host") ? vm["host"].as<std::string>() : "",
      vm.count("port") ? vm["port"].as<std::size_t>() : 0);

  std::string db_host = vm["host"].as<std::string>();
  int db_port = vm["port"].as<std::size_t>();

  planning_models_loader::KinematicModelLoader kinematic_model_loader("robot_description"); /** Used to load the robot model */
  kinematic_model::KinematicModelPtr kinematic_model = kinematic_model_loader.getModel();

  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor;
  planning_scene_monitor.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description"));

  unsigned int proc = 0;
  for (int i = 1 ; i < argc ; ++i)
  {
    if (bc.readOptions(argv[i]))
    {
      std::stringstream ss;
      bc.printOptions(ss);
      ROS_INFO("Calling kinematic reachability benchmark with options:\n%s\n", ss.str().c_str());
      proc++;

      std::string group_name, frame_id, scene_name;

      scene_name = bc.getOptions().scene;
      group_name = bc.getOptions().group_override;
      frame_id = bc.getOptions().planning_frame;
      if (bc.getOptions().output.empty())
      {
        ROS_ERROR("output parameter cannot be empty. Review your config file");
        return 0;
      }

      moveit_warehouse::PlanningSceneStorage pss(db_host, db_port);
      moveit_warehouse::PlanningSceneWithMetadata pswm;
      pss.getPlanningScene(pswm, scene_name);
      moveit_msgs::PlanningScene ps = static_cast<const moveit_msgs::PlanningScene&>(*pswm);
      planning_scene_monitor->getPlanningScene()->usePlanningSceneMsg(ps);

      ROS_INFO_STREAM("Processing scene " << planning_scene_monitor->getPlanningScene()->getName());

      kinematics_constraint_aware::KinematicsConstraintAwarePtr kinematics_constraint_aware;
      kinematics_constraint_aware.reset(new kinematics_constraint_aware::KinematicsConstraintAware(kinematic_model, group_name));

      moveit_warehouse::ConstraintsStorage cs(db_host, db_port);
      std::vector<std::string> constraints_names;
      cs.getKnownConstraints(bc.getOptions().goal_regex, constraints_names);
      for (int c = 0; c < constraints_names.size(); c++)
      {
        moveit_warehouse::ConstraintsWithMetadata cwm;
        cs.getConstraints(cwm, constraints_names[c]);

        geometry_msgs::PoseStamped ik_pose;
        ik_pose.header.frame_id = frame_id;
        ik_pose.pose.position.x = cwm->position_constraints[0].constraint_region.primitive_poses[0].position.x;
        ik_pose.pose.position.y = cwm->position_constraints[0].constraint_region.primitive_poses[0].position.y;
        ik_pose.pose.position.z = cwm->position_constraints[0].constraint_region.primitive_poses[0].position.z;
        ik_pose.pose.orientation.x = cwm->orientation_constraints[0].orientation.x;
        ik_pose.pose.orientation.y = cwm->orientation_constraints[0].orientation.y;
        ik_pose.pose.orientation.z = cwm->orientation_constraints[0].orientation.z;
        ik_pose.pose.orientation.w = cwm->orientation_constraints[0].orientation.w;

        moveit_msgs::MoveItErrorCodes error_code;
        moveit_msgs::RobotState solution;

        //Fill the IK Request msg
        moveit_msgs::GetConstraintAwarePositionIK::Request request;
        moveit_msgs::GetConstraintAwarePositionIK::Response response;

        kinematic_model::KinematicModelConstPtr kinematic_model = kinematics_constraint_aware->getKinematicModel();
        kinematic_state::KinematicState kinematic_state(kinematic_model);
        const kinematic_model::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup(group_name);
        kinematic_state::JointStateGroup* joint_state_group = kinematic_state.getJointStateGroup(group_name);
        joint_state_group->setToRandomValues();

        request.timeout = ros::Duration(0.5);
        request.ik_request.robot_state.joint_state.name = joint_model_group->getJointModelNames();
        joint_state_group->getVariableValues(request.ik_request.robot_state.joint_state.position);
        request.ik_request.group_name = group_name;
        request.ik_request.pose_stamped = ik_pose;

        //Compute IK
        kinematics_constraint_aware->getIK(planning_scene_monitor->getPlanningScene(),request,response);

        std::string filename = bc.getOptions().output + "_kinematics." + boost::lexical_cast<std::string>(c+1) + ".log";
        std::ofstream out(filename.c_str());

        out << "REACHABLE" << std::endl;
        if (response.error_code.val == response.error_code.SUCCESS)
        {
          ROS_INFO_STREAM(" Goal " << constraints_names[c] << " SUCCESS!");
          out << "1" << std::endl;
        }
        else
        {
          ROS_INFO_STREAM(" Goal " << constraints_names[c] << " FAIL with code " << (int)response.error_code.val);
          out << "0" << std::endl;
        }
      }
    }
  }
  ROS_INFO("Processed %u benchmark configuration files", proc);

  return(0);
}
<|endoftext|>


<commit_before>#include "consts.h"
#include "circuits/circuit.h"
#include "circuits/element.h"
#include "matrix/matrix.h"
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <sstream>
#include <cstdlib>

using namespace std;

Circuit::Circuit():
     variablesList(MAX_NODES+1),
     netlist(MAX_ELEMS)
{
}

Circuit::Circuit(int numElements,
                 int numNodes,
                 int numVariables,
                 vector<Element> &netlist):
     variablesList(MAX_NODES+1),
     netlist(netlist),
     numElements(numElements),
     numNodes(numNodes),
     numVariables(numVariables)
{
}

Circuit::Circuit(ifstream &netlistFile):
     variablesList(MAX_NODES+1),
     netlist(MAX_ELEMS)
{
    numElements=0;
    numNodes=0;
    numVariables=0;
    // XXX Magic! Really important!
    // Ground node!
    variablesList[0] = "0";

    string netlistLine;
    bool isValidElement;
    char netlistLinePrefix;
    getline(netlistFile, netlistLine);
    title = netlistLine;
    while (getline(netlistFile, netlistLine)) {
        netlistLinePrefix = netlistLine[0];
        isValidElement = Element::isValidElement(netlistLinePrefix);

        if (isValidElement){
            // Element is valid!
            numElements++; /* XXX Starts from netlist[1] */
            if (numElements>MAX_ELEMS) {
                cout << "Invalid number of elements. Maximum number of elements is " << MAX_ELEMS << endl;
                #if defined (WIN32) || defined(_WIN32)
                cout << endl << "Press any key to exit...";
                cin.get();
                cin.get();
                #endif
                exit(EXIT_FAILURE);

            }
            netlist[numElements] = Element(netlistLine, numNodes, variablesList);
        }
        else if (netlistLinePrefix == '.'){
            stringstream ss(netlistLine);
            string name;
            ss >> name;
            if ( !name.compare(".TRAN") ){
                string analysisType; //value not used, always equal to BE
                ss >> finalTime >> step >> analysisType >> numInternalSteps;
            }else{
                cout << "Invalid line: " << netlistLine << endl;
                #if defined (WIN32) || defined(_WIN32)
                cout << endl << "Press any key to exit...";
                cin.get();
                cin.get();
                #endif
                exit(EXIT_FAILURE);
            }
        }
        else if (netlistLinePrefix != '*') {
            // Not a comment, not a valid element...
            // Invalid line!
            cout << "Invalid line: " << netlistLine << endl;
            #if defined (WIN32) || defined(_WIN32)
            cout << endl << "Press any key to exit...";
            cin.get();
            cin.get();
            #endif
            exit(EXIT_FAILURE);
        }// Ignores comments!
    }
    cout << endl;
    // Figured out elements and all nodes

    // Now, will add extra current variables!
    numVariables = numNodes;
    for(int i=1; i<=numElements; i++){
        netlist[i].addCurrentVariables(numVariables, variablesList);
    }
    if (numVariables > MAX_NODES) {
        cout << "Extra current variables exceeded maximum number of variables: " << MAX_NODES << endl;
        #if defined (WIN32) || defined(_WIN32)
        cout << endl << "Press any key to exit...";
        cin.get();
        cin.get();
        #endif
        exit(EXIT_FAILURE);
    }
}

void Circuit::printVariables(){
    for (int i=0; i<=numVariables; i++){
        cout << i << " -> " << variablesList[i] << endl;
    }
    cout << endl;
}

void Circuit::printSummary(){
    cout << "# nodes:\t" << numNodes << endl;
    cout << "# variables:\t" << numVariables << endl;
    cout << "# elements:\t" << numElements << endl;
    cout << endl;
}

void Circuit::applyStamps(double Yn[MAX_NODES+1][MAX_NODES+2],
                          double (&previousSolution)[MAX_NODES+1],
                          double t,
                          double (&lastStepSolution)[MAX_NODES+1]){
    Element element;
    for (int i=1; i<=numElements; i++) {
        element = netlist[i];
        // Will pass previousSolution in the near future...
        element.applyStamp(Yn, numVariables, previousSolution, t, step, lastStepSolution);
        #ifdef DEBUG
        cout << "System after stamp of " << element.getName() << endl;
        print(numVariables, Yn);
        #endif

    }
}

void Circuit::printSolution(double Yn[MAX_NODES+1][MAX_NODES+2]){
    for (int i=1; i<=numVariables; i++) {
        cout << variablesList[i] << ":\t" << Yn[i][numVariables+1] << endl;
    }
}

void Circuit::writeSolutionsHeader(ofstream &file){
    for (int i = 0; i <= numVariables; i++){
        if (i == 0)
            file << "t ";
        else
            file << variablesList[i] << " ";
    }
    file << endl;
}

void Circuit::appendSolutionToFile(ofstream &file, double solution[MAX_NODES+1], double t){
    file << t << " ";
    for (int i=1; i<= numVariables; i++)
        file << solution[i] << " ";
    file << std::endl;
}

int Circuit::getNumElements(){
    return numElements;
};

int Circuit::getNumNodes(){
    return numNodes;
};

int Circuit::getNumVariables(){
    return numVariables;
};

double Circuit::getStep(){
    return step;
};

int Circuit::getNumInternalSteps(){
    return numInternalSteps;
};

double Circuit::getFinalTime(){
    return finalTime;
};

<commit_msg>Fixes support for purely bias analysis<commit_after>#include "consts.h"
#include "circuits/circuit.h"
#include "circuits/element.h"
#include "matrix/matrix.h"
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <sstream>
#include <cstdlib>

using namespace std;

Circuit::Circuit():
     variablesList(MAX_NODES+1),
     netlist(MAX_ELEMS)
{
}

Circuit::Circuit(int numElements,
                 int numNodes,
                 int numVariables,
                 vector<Element> &netlist):
     variablesList(MAX_NODES+1),
     netlist(netlist),
     numElements(numElements),
     numNodes(numNodes),
     numVariables(numVariables)
{
}

Circuit::Circuit(ifstream &netlistFile):
     variablesList(MAX_NODES+1),
     netlist(MAX_ELEMS)
{
    numElements=0;
    numNodes=0;
    numVariables=0;
    // XXX Magic! Really important!
    // Ground node!
    variablesList[0] = "0";

    string netlistLine;
    bool isValidElement;
    char netlistLinePrefix;
    getline(netlistFile, netlistLine);
    title = netlistLine;
    while (getline(netlistFile, netlistLine)) {
        netlistLinePrefix = netlistLine[0];
        isValidElement = Element::isValidElement(netlistLinePrefix);

        if (isValidElement){
            // Element is valid!
            numElements++; /* XXX Starts from netlist[1] */
            if (numElements>MAX_ELEMS) {
                cout << "Invalid number of elements. Maximum number of elements is " << MAX_ELEMS << endl;
                #if defined (WIN32) || defined(_WIN32)
                cout << endl << "Press any key to exit...";
                cin.get();
                cin.get();
                #endif
                exit(EXIT_FAILURE);

            }
            netlist[numElements] = Element(netlistLine, numNodes, variablesList);
        }
        else if (netlistLinePrefix == '.'){
            stringstream ss(netlistLine);
            string name;
            ss >> name;
            if ( !name.compare(".TRAN") ){
                string analysisType; //value not used, always equal to BE
                ss >> finalTime >> step >> analysisType >> numInternalSteps;
            }else{
                cout << "Invalid line: " << netlistLine << endl;
                #if defined (WIN32) || defined(_WIN32)
                cout << endl << "Press any key to exit...";
                cin.get();
                cin.get();
                #endif
                exit(EXIT_FAILURE);
            }
        }
        else if (netlistLinePrefix != '*') {
            // Not a comment, not a valid element...
            // Invalid line!
            cout << "Invalid line: " << netlistLine << endl;
            #if defined (WIN32) || defined(_WIN32)
            cout << endl << "Press any key to exit...";
            cin.get();
            cin.get();
            #endif
            exit(EXIT_FAILURE);
        }// Ignores comments!
    }
    cout << endl;
    // Figured out elements and all nodes

    // Now, will add extra current variables!
    numVariables = numNodes;
    for(int i=1; i<=numElements; i++){
        netlist[i].addCurrentVariables(numVariables, variablesList);
    }
    if (numVariables > MAX_NODES) {
        cout << "Extra current variables exceeded maximum number of variables: " << MAX_NODES << endl;
        #if defined (WIN32) || defined(_WIN32)
        cout << endl << "Press any key to exit...";
        cin.get();
        cin.get();
        #endif
        exit(EXIT_FAILURE);
    }
}

void Circuit::printVariables(){
    for (int i=0; i<=numVariables; i++){
        cout << i << " -> " << variablesList[i] << endl;
    }
    cout << endl;
}

void Circuit::printSummary(){
    cout << "# nodes:\t" << numNodes << endl;
    cout << "# variables:\t" << numVariables << endl;
    cout << "# elements:\t" << numElements << endl;
    cout << endl;
}

void Circuit::applyStamps(double Yn[MAX_NODES+1][MAX_NODES+2],
                          double (&previousSolution)[MAX_NODES+1],
                          double t,
                          double (&lastStepSolution)[MAX_NODES+1]){
    Element element;
    for (int i=1; i<=numElements; i++) {
        element = netlist[i];
        // Will pass previousSolution in the near future...
        element.applyStamp(Yn, numVariables, previousSolution, t, step, lastStepSolution);
        #ifdef DEBUG
        cout << "System after stamp of " << element.getName() << endl;
        print(numVariables, Yn);
        #endif

    }
}

void Circuit::printSolution(double Yn[MAX_NODES+1][MAX_NODES+2]){
    for (int i=1; i<=numVariables; i++) {
        cout << variablesList[i] << ":\t" << Yn[i][numVariables+1] << endl;
    }
}

void Circuit::writeSolutionsHeader(ofstream &file){
    for (int i = 0; i <= numVariables; i++){
        if (i == 0)
            file << "t ";
        else
            file << variablesList[i] << " ";
    }
    file << endl;
}

void Circuit::appendSolutionToFile(ofstream &file, double solution[MAX_NODES+1], double t){
    file << t << " ";
    for (int i=1; i<= numVariables; i++)
        file << solution[i] << " ";
    file << std::endl;
}

int Circuit::getNumElements(){
    return numElements;
};

int Circuit::getNumNodes(){
    return numNodes;
};

int Circuit::getNumVariables(){
    return numVariables;
};

double Circuit::getStep(){
    if (step<0)
        step = 0;
    return step;
};

int Circuit::getNumInternalSteps(){
    if (numInternalSteps<0)
        numInternalSteps = 1;
    return numInternalSteps;
};

double Circuit::getFinalTime(){
    if (finalTime<0)
        finalTime = 0;
    return finalTime;
};

<|endoftext|>


<commit_before>/*
 * FirmThresholdCostFnProbe.hpp
 *
 *  Created on: Aug 14, 2015
 *      Author: pschultz
 */

#ifndef FIRMTHRESHOLDCOSTFNPROBE_HPP_
#define FIRMTHRESHOLDCOSTFNPROBE_HPP_

#include "AbstractNormProbe.hpp"

namespace PV {

/**
 * A class for computing the cost function corresponding to a transfer function
 * intermediate between soft threshold and hard threshold:
 * If |V|<VThresh, activity is zero.  If |V|>VThresh+VWidth, activity is the same as V.
 * Otherwise, activity is sgn(V)*(|V|-VThresh)*(VThresh/VWidth+1)
 * Note that this defines a continuous, pointwise linear function of the membrane potential,
 * with breaks where |V|=VThresh and |V|=VThresh+VWidth.
 * Also, as VWidth->0, the transfer function approaches the hard threshold case, and as
 * VWidth->infinity, the transfer function approaches the soft threshold case.
 *
 * The cost function for a layer with activities y_i is given by the sum of C(y_i),
 * where C(y_i) = (VThresh+VWidth)/2 if |y_i|>=VThresh+VWidth, and
 * C(y_i) = y_i*(1-y_i/(VThresh+VWidth)/2) if |y_i|<VThresh+VWidth.
 */
class FirmThresholdCostFnProbe : public AbstractNormProbe {
public:
   FirmThresholdCostFnProbe(const char * probeName, HyPerCol * hc);
   virtual ~FirmThresholdCostFnProbe();

   virtual int communicateInitInfo();

protected:
   FirmThresholdCostFnProbe();
   int initFirmThresholdCostFnProbe(const char * probeName, HyPerCol * hc);
   virtual double getValueInternal(double timevalue, int index);
   virtual int ioParamsFillGroup(enum ParamsIOFlag ioFlag);

   /**
    * List of parameters for the FirmThresholdCostFnProbe class
    * @name FirmThresholdCostFnProbe Parameters
    * @{
    */

   /**
    * @brief VThresh: The threshold where the transfer function
    * returns 0 if |V|<VThresh.  If the target layer is an ANNLayer or one
    * of its subclasses, the default is the target layer's VThresh;
    * otherwise, the default is zero.
    */
   virtual void ioParam_VThresh(enum ParamsIOFlag ioFlag);

   /**
    * @brief VWidth: The width of the interval over which the transfer function
    * changes from hard to soft threshold.  If the target layer is an ANNLayer or
    * one of its subclasses, the default is the target layer's VWidth; otherwise,
    * the default is zero.
    */
   virtual void ioParam_VWidth(enum ParamsIOFlag ioFlag);
   /** @} */

   /**
    * Overrides AbstractNormProbe::setNormDescription() to set normDescription to "Cost function".
    * Return values and errno are set by a call to setNormDescriptionToString.
    */
   virtual int setNormDescription();

private:
   int initFirmThresholdCostFnProbe_base();

// Member variables
protected:
   pvpotentialdata_t VThresh;
   pvpotentialdata_t VWidth;
}; // end class FirmThresholdCostFnProbe

}  // end namespace PV

#endif /* FIRMTHRESHOLDCOSTFNPROBE_HPP_ */
<commit_msg>Corrects description of FirmThresholdCostFnProbe cost function for negative activities.<commit_after>/*
 * FirmThresholdCostFnProbe.hpp
 *
 *  Created on: Aug 14, 2015
 *      Author: pschultz
 */

#ifndef FIRMTHRESHOLDCOSTFNPROBE_HPP_
#define FIRMTHRESHOLDCOSTFNPROBE_HPP_

#include "AbstractNormProbe.hpp"

namespace PV {

/**
 * A class for computing the cost function corresponding to a transfer function
 * intermediate between soft threshold and hard threshold:
 * If |V|<VThresh, activity is zero.  If |V|>VThresh+VWidth, activity is the same as V.
 * Otherwise, activity is sgn(V)*(|V|-VThresh)*(VThresh/VWidth+1)
 * Note that this defines a continuous, pointwise linear function of the membrane potential,
 * with breaks where |V|=VThresh and |V|=VThresh+VWidth.
 * Also, as VWidth->0, the transfer function approaches the hard threshold case, and as
 * VWidth->infinity, the transfer function approaches the soft threshold case.
 *
 * The cost function for a layer with activities y_i is given by the sum of C(y_i),
 * where C(y_i) = (VThresh+VWidth)/2 if |y_i|>=VThresh+VWidth, and
 * C(y_i) = |y_i|*(1-|y_i|/(VThresh+VWidth)/2) if |y_i|<VThresh+VWidth.
 */
class FirmThresholdCostFnProbe : public AbstractNormProbe {
public:
   FirmThresholdCostFnProbe(const char * probeName, HyPerCol * hc);
   virtual ~FirmThresholdCostFnProbe();

   virtual int communicateInitInfo();

protected:
   FirmThresholdCostFnProbe();
   int initFirmThresholdCostFnProbe(const char * probeName, HyPerCol * hc);
   virtual double getValueInternal(double timevalue, int index);
   virtual int ioParamsFillGroup(enum ParamsIOFlag ioFlag);

   /**
    * List of parameters for the FirmThresholdCostFnProbe class
    * @name FirmThresholdCostFnProbe Parameters
    * @{
    */

   /**
    * @brief VThresh: The threshold where the transfer function
    * returns 0 if |V|<VThresh.  If the target layer is an ANNLayer or one
    * of its subclasses, the default is the target layer's VThresh;
    * otherwise, the default is zero.
    */
   virtual void ioParam_VThresh(enum ParamsIOFlag ioFlag);

   /**
    * @brief VWidth: The width of the interval over which the transfer function
    * changes from hard to soft threshold.  If the target layer is an ANNLayer or
    * one of its subclasses, the default is the target layer's VWidth; otherwise,
    * the default is zero.
    */
   virtual void ioParam_VWidth(enum ParamsIOFlag ioFlag);
   /** @} */

   /**
    * Overrides AbstractNormProbe::setNormDescription() to set normDescription to "Cost function".
    * Return values and errno are set by a call to setNormDescriptionToString.
    */
   virtual int setNormDescription();

private:
   int initFirmThresholdCostFnProbe_base();

// Member variables
protected:
   pvpotentialdata_t VThresh;
   pvpotentialdata_t VWidth;
}; // end class FirmThresholdCostFnProbe

}  // end namespace PV

#endif /* FIRMTHRESHOLDCOSTFNPROBE_HPP_ */
<|endoftext|>


<commit_before>#include <gtest/gtest.h>
#include "stan/prob/distributions/multivariate/continuous/gaussian_dlm.hpp"

using Eigen::Dynamic;
using Eigen::Matrix;
using Eigen::MatrixXd;

using boost::math::policies::policy;
using boost::math::policies::evaluation_error;
using boost::math::policies::domain_error;
using boost::math::policies::overflow_error;
using boost::math::policies::domain_error;
using boost::math::policies::pole_error;
using boost::math::policies::errno_on_error;
using stan::prob::gaussian_dlm_log;

// NOTE: what does this do?
typedef policy<
  domain_error<errno_on_error>,
  pole_error<errno_on_error>,
  overflow_error<errno_on_error>,
  evaluation_error<errno_on_error>
  > errno_policy;

TEST(ProbDistributionsGaussianDLM,LoglikeUU) {
  Matrix<double, Dynamic, Dynamic> FF(1, 1);
  FF << 0.585528817843856;
  Matrix<double, Dynamic, Dynamic> GG(1, 1);
  GG << -0.109303314681054;
  Matrix<double, Dynamic, Dynamic> V(1, 1);
  V << 2.25500747900521;
  Matrix<double, Dynamic, Dynamic> W(1, 1);
  W << 0.461487989960454;
  Matrix<double, Dynamic, Dynamic> y(1, 10);
  y << 0.435794268894196, 1.22755796113506, 0.193264580222731, 1.76021889445093, 1.6470149263738, 0.059988547306031, -0.4980979884018, 1.77794742623532, -0.435458536090977, 1.1733293160216;
  double ll_expected = -21.0504107180498;

  double lp_ref = gaussian_dlm_log(y, FF, GG, V, W);
  EXPECT_FLOAT_EQ(lp_ref, ll_expected);
}

TEST(ProbDistributionsGaussianDLM,LoglikeMM) {
  Matrix<double, Dynamic, Dynamic> FF(2, 3);
  FF << 0.585528817843856,0.709466017509524,-0.109303314681054,-0.453497173462763,0.605887455840394,-1.81795596770373;
  Matrix<double, Dynamic, Dynamic> GG(2, 2);
  GG << 0.520216457554957,0.816899839520583,-0.750531994502331,-0.886357521243213;
  Matrix<double, Dynamic, Dynamic> V(3, 3);
  V << 7.19105866377728,-0.311731853764732,4.87333111936296,-0.311731853764732,3.27048576782842,0.457616661474554,4.87333111936296,0.457616661474554,5.86564522448303;
  Matrix<double, Dynamic, Dynamic> W(2, 2);
  W << 2.24277594357501,-1.65863136283477,-1.65863136283477,6.69010664813895;
  Matrix<double, Dynamic, Dynamic> y(3, 10);
  y << 0.663454889128383, 1.84506567787219, -0.887054506009854, -2.41521382778332, 3.70875481434455, -4.55234499588754, -0.605128307725781, 1.37074056153151, -2.12107763131894, 2.24236743746261, 0.386569006173759, 3.91825641599579, 3.33368860016417, -0.469849046245645, -2.19806833413509, -1.45754611697235, 2.32377488296413, 4.8514565931381, -3.3192402890674, -4.86325482585216, -1.29504029175967, -1.15116681671907, -1.20273925104474, -2.53276712020605, -1.06306293348278, -2.38535359596865, 0.693389700056057, -3.25137196974247, 1.32287059182635, 0.745762191888096;
  double ll_expected = -90.0322237154493;

  // the error adds up in the multivariate version due to the inversion
  double lp_ref = gaussian_dlm_log(y, FF, GG, V, W);
  EXPECT_NEAR(lp_ref, ll_expected, 1e-4);
}

TEST(ProbDistributionsGaussianDLM,LoglikeUUSeq) {
  Matrix<double, Dynamic, Dynamic> FF(1, 1);
  FF << 0.585528817843856;
  Matrix<double, Dynamic, Dynamic> GG(1, 1);
  GG << -0.109303314681054;
  Matrix<double, Dynamic, Dynamic> W(1, 1);
  W << 0.461487989960454;
  Matrix<double, Dynamic, 1> V(1);
  V << 2.25500747900521;
  Matrix<double, Dynamic, Dynamic> y(1, 10);
  y << -30.2383237005506, 4.58034073011305, -0.173205689832086, 1.80027530969998, 1.64263662741284, 0.0604671098951288, -0.49815029687907, 1.77795314372528, -0.435459161031581, 1.17332938432968;
  double ll_expected = -21.0616654161768;
  double lp_ref = gaussian_dlm_log(y, FF, GG, V, W);
  EXPECT_FLOAT_EQ(lp_ref, ll_expected);
}

TEST(ProbDistributionsGaussianDLM,LoglikeMMSeq) {
  Matrix<double, Dynamic, Dynamic> FF(2, 3);
 FF << 0.585528817843856,0.709466017509524,-0.109303314681054,-0.453497173462763,0.605887455840394,-1.81795596770373;
 Matrix<double, Dynamic, Dynamic> GG(2, 2);
 GG << 0.520216457554957,0.816899839520583,-0.750531994502331,-0.886357521243213;
 Matrix<double, Dynamic, Dynamic> W(2, 2);
 W << 2.24277594357501,-1.65863136283477,-1.65863136283477,6.69010664813895;
 Matrix<double, Dynamic, 1> V(3);
 V << 7.19105866377728,3.27048576782842,5.86564522448303;
 Matrix<double, Dynamic, Dynamic> y(3, 10);
 y << -645.337793038888, -85.2449543825825, -1027.80947332792, 115.550478561317, 83.7383685001328, 79.50821971367, 52.2957712041988, -15.0415380740324, 124.483546197942, -34.6937213615964, -5.72301661410856, -55.3761417070498, 8.80290702350217, 4.44692709691049, -2.07768404916278, 1.57843833028236, 1.23783669851953, 15.5659876348306, -6.86188181853719, -5.50621793641665, -3.62888602818304, -0.145891471793544, -0.825028473757867, -2.52251372025581, -1.19867033251869, -2.47536209021952, 1.41122996270707, -5.95624742000524, 0.832231787736836, 2.38404919543544;
 double ll_expected = -82647.3683004951;
 double lp_ref = gaussian_dlm_log(y, FF, GG, V, W);
 EXPECT_FLOAT_EQ(lp_ref, ll_expected);
}

TEST(ProbDistributionsGaussianDLM,Sizing) {
  // consistent matrices
  Matrix<double, Dynamic, Dynamic> FF = MatrixXd::Random(2, 3);
  Matrix<double, Dynamic, Dynamic> GG = MatrixXd::Random(2, 2);
  Matrix<double, Dynamic, Dynamic> V = MatrixXd::Identity(3, 3);
  Matrix<double, Dynamic, Dynamic> W = MatrixXd::Identity(2, 2);
  Matrix<double, Dynamic, Dynamic> y = MatrixXd::Identity(3, 5);

  Matrix<double, Dynamic, Dynamic> bad_FF_1 = MatrixXd::Random(4, 3);
  EXPECT_ANY_THROW(gaussian_dlm_log(y, bad_FF_1, GG, V, W));

  Matrix<double, Dynamic, Dynamic> bad_FF_2 = MatrixXd::Random(2, 4);
  EXPECT_ANY_THROW(gaussian_dlm_log(y, bad_FF_2, GG, V, W));

  Matrix<double, Dynamic, Dynamic> bad_GG_1 = MatrixXd::Random(3, 3);
  EXPECT_ANY_THROW(gaussian_dlm_log(y, FF, bad_GG_1, V, W));
  Matrix<double, Dynamic, Dynamic> bad_GG_2 = MatrixXd::Random(2, 3);
  EXPECT_ANY_THROW(gaussian_dlm_log(y, FF, bad_GG_2, V, W));

  //Not symmetric
  V(0, 2) = 1;
  EXPECT_ANY_THROW(gaussian_dlm_log(y, FF, GG, V, W));
  // negative
  V(0, 2) = -1;
  EXPECT_ANY_THROW(gaussian_dlm_log(y, FF, GG, V, W));

  // wrong size
  Matrix<double, Dynamic, Dynamic> V1 = MatrixXd::Identity(2, 2);
  EXPECT_ANY_THROW(gaussian_dlm_log(y, FF, GG, V1, W));
  // not square
  Matrix<double, Dynamic, Dynamic> V2 = MatrixXd::Identity(2, 3);
  EXPECT_ANY_THROW(gaussian_dlm_log(y, FF, GG, V2, W));

  //Not symmetric
  W(0, 1) = 1;
  EXPECT_ANY_THROW(gaussian_dlm_log(y, FF, GG, V, W));
  // negative
  W(0, 1) = -1;
  EXPECT_ANY_THROW(gaussian_dlm_log(y, FF, GG, V, W));
  // wrong size
  Matrix<double, Dynamic, Dynamic> W1 = MatrixXd::Identity(3, 3);
  EXPECT_ANY_THROW(gaussian_dlm_log(y, FF, GG, V, W1));
  // not square
  Matrix<double, Dynamic, Dynamic> W2 = MatrixXd::Identity(2, 3);
  // EXPECT_ANY_THROW(gaussian_dlm_log(y, FF, GG, V, W2));

}
<commit_msg>updating tests<commit_after>#include <gtest/gtest.h>
#include "stan/prob/distributions/multivariate/continuous/gaussian_dlm.hpp"

using Eigen::Dynamic;
using Eigen::Matrix;
using Eigen::MatrixXd;

using boost::math::policies::policy;
using boost::math::policies::evaluation_error;
using boost::math::policies::domain_error;
using boost::math::policies::overflow_error;
using boost::math::policies::domain_error;
using boost::math::policies::pole_error;
using boost::math::policies::errno_on_error;
using stan::prob::gaussian_dlm_log;

// NOTE: what does this do?
typedef policy<
  domain_error<errno_on_error>,
  pole_error<errno_on_error>,
  overflow_error<errno_on_error>,
  evaluation_error<errno_on_error>
  > errno_policy;

TEST(ProbDistributionsGaussianDLM,LoglikeUU) {
  Matrix<double, Dynamic, Dynamic> FF(1, 1);
  FF << 0.585528817843856;
  Matrix<double, Dynamic, Dynamic> GG(1, 1);
  GG << -0.109303314681054;
  Matrix<double, Dynamic, Dynamic> V(1, 1);
  V << 2.25500747900521;
  Matrix<double, Dynamic, Dynamic> W(1, 1);
  W << 0.461487989960454;
  Matrix<double, Dynamic, Dynamic> y(1, 10);
  y << 0.435794268894196, 1.22755796113506, 0.193264580222731, 1.76021889445093, 1.6470149263738, 0.059988547306031, -0.4980979884018, 1.77794742623532, -0.435458536090977, 1.1733293160216;
  double ll_expected = -21.0504107180498;

  double lp_ref = gaussian_dlm_log(y, FF, GG, V, W);
  EXPECT_FLOAT_EQ(lp_ref, ll_expected);
}

TEST(ProbDistributionsGaussianDLM,LoglikeMM) {
  Matrix<double, Dynamic, Dynamic> FF(2, 3);
  FF << 0.585528817843856,0.709466017509524,-0.109303314681054,-0.453497173462763,0.605887455840394,-1.81795596770373;
  Matrix<double, Dynamic, Dynamic> GG(2, 2);
  GG << 0.520216457554957,0.816899839520583,-0.750531994502331,-0.886357521243213;
  Matrix<double, Dynamic, Dynamic> V(3, 3);
  V << 7.19105866377728,-0.311731853764732,4.87333111936296,-0.311731853764732,3.27048576782842,0.457616661474554,4.87333111936296,0.457616661474554,5.86564522448303;
  Matrix<double, Dynamic, Dynamic> W(2, 2);
  W << 2.24277594357501,-1.65863136283477,-1.65863136283477,6.69010664813895;
  Matrix<double, Dynamic, Dynamic> y(3, 10);
  y << 0.663454889128383, 1.84506567787219, -0.887054506009854, -2.41521382778332, 3.70875481434455, -4.55234499588754, -0.605128307725781, 1.37074056153151, -2.12107763131894, 2.24236743746261, 0.386569006173759, 3.91825641599579, 3.33368860016417, -0.469849046245645, -2.19806833413509, -1.45754611697235, 2.32377488296413, 4.8514565931381, -3.3192402890674, -4.86325482585216, -1.29504029175967, -1.15116681671907, -1.20273925104474, -2.53276712020605, -1.06306293348278, -2.38535359596865, 0.693389700056057, -3.25137196974247, 1.32287059182635, 0.745762191888096;
  double ll_expected = -90.0322237154493;

  // the error adds up in the multivariate version due to the inversion
  double lp_ref = gaussian_dlm_log(y, FF, GG, V, W);
  EXPECT_NEAR(lp_ref, ll_expected, 1e-4);
}

TEST(ProbDistributionsGaussianDLM,LoglikeUUSeq) {
  Matrix<double, Dynamic, Dynamic> FF(1, 1);
  FF << 0.585528817843856;
  Matrix<double, Dynamic, Dynamic> GG(1, 1);
  GG << -0.109303314681054;
  Matrix<double, Dynamic, Dynamic> W(1, 1);
  W << 0.461487989960454;
  Matrix<double, Dynamic, 1> V(1);
  V << 2.25500747900521;
  Matrix<double, Dynamic, Dynamic> y(1, 10);
  y << -30.2383237005506, 4.58034073011305, -0.173205689832086, 1.80027530969998, 1.64263662741284, 0.0604671098951288, -0.49815029687907, 1.77795314372528, -0.435459161031581, 1.17332938432968;
  double ll_expected = -21.0616654161768;
  double lp_ref = gaussian_dlm_log(y, FF, GG, V, W);
  EXPECT_FLOAT_EQ(lp_ref, ll_expected);
}

TEST(ProbDistributionsGaussianDLM,LoglikeMMSeq) {
  Matrix<double, Dynamic, Dynamic> FF(2, 3);
 FF << 0.585528817843856,0.709466017509524,-0.109303314681054,-0.453497173462763,0.605887455840394,-1.81795596770373;
 Matrix<double, Dynamic, Dynamic> GG(2, 2);
 GG << 0.520216457554957,0.816899839520583,-0.750531994502331,-0.886357521243213;
 Matrix<double, Dynamic, Dynamic> W(2, 2);
 W << 2.24277594357501,-1.65863136283477,-1.65863136283477,6.69010664813895;
 Matrix<double, Dynamic, 1> V(3);
 V << 7.19105866377728,3.27048576782842,5.86564522448303;
 Matrix<double, Dynamic, Dynamic> y(3, 10);
 y << -645.337793038888, -85.2449543825825, -1027.80947332792, 115.550478561317, 83.7383685001328, 79.50821971367, 52.2957712041988, -15.0415380740324, 124.483546197942, -34.6937213615964, -5.72301661410856, -55.3761417070498, 8.80290702350217, 4.44692709691049, -2.07768404916278, 1.57843833028236, 1.23783669851953, 15.5659876348306, -6.86188181853719, -5.50621793641665, -3.62888602818304, -0.145891471793544, -0.825028473757867, -2.52251372025581, -1.19867033251869, -2.47536209021952, 1.41122996270707, -5.95624742000524, 0.832231787736836, 2.38404919543544;
 double ll_expected = -82647.3683004951;
 double lp_ref = gaussian_dlm_log(y, FF, GG, V, W);
 EXPECT_FLOAT_EQ(lp_ref, ll_expected);
}

TEST(ProbDistributionsGaussianDLM,Sizing) {
  // consistent matrices
  Matrix<double, Dynamic, Dynamic> FF = MatrixXd::Random(2, 3);
  Matrix<double, Dynamic, Dynamic> GG = MatrixXd::Random(2, 2);
  Matrix<double, Dynamic, Dynamic> V = MatrixXd::Identity(3, 3);
  Matrix<double, Dynamic, Dynamic> W = MatrixXd::Identity(2, 2);
  Matrix<double, Dynamic, Dynamic> y = MatrixXd::Identity(3, 5);

  // Check F
  Matrix<double, Dynamic, Dynamic> bad_FF_1 = MatrixXd::Random(4, 3);
  EXPECT_THROW(gaussian_dlm_log(y, bad_FF_1, GG, V, W), std::domain_error);

  Matrix<double, Dynamic, Dynamic> bad_FF_2 = MatrixXd::Random(2, 4);
  EXPECT_THROW(gaussian_dlm_log(y, bad_FF_2, GG, V, W), std::domain_error);

  // Check G
  Matrix<double, Dynamic, Dynamic> bad_GG_1 = MatrixXd::Random(3, 3);
  EXPECT_THROW(gaussian_dlm_log(y, FF, bad_GG_1, V, W), std::domain_error);
  Matrix<double, Dynamic, Dynamic> bad_GG_2 = MatrixXd::Random(2, 3);
  EXPECT_THROW(gaussian_dlm_log(y, FF, bad_GG_2, V, W), std::domain_error);

  // Check V
  //Not symmetric
  V(0, 2) = 1;
  EXPECT_THROW(gaussian_dlm_log(y, FF, GG, V, W), std::domain_error);
  // negative
  V(0, 2) = -1;
  EXPECT_THROW(gaussian_dlm_log(y, FF, GG, V, W), std::domain_error);
  // wrong size
  Matrix<double, Dynamic, Dynamic> V1 = MatrixXd::Identity(2, 2);
  EXPECT_THROW(gaussian_dlm_log(y, FF, GG, V1, W), std::domain_error);
  // wrong size vector
  Matrix<double, Dynamic, 1L> V2(4);
  EXPECT_THROW(gaussian_dlm_log(y, FF, GG, V2, W), std::domain_error);
  // not square
  Matrix<double, Dynamic, Dynamic> V3 = MatrixXd::Identity(2, 3);
  EXPECT_THROW(gaussian_dlm_log(y, FF, GG, V3, W), std::domain_error);

  //Not symmetric
  W(0, 1) = 1;
  EXPECT_THROW(gaussian_dlm_log(y, FF, GG, V, W), std::domain_error);
  // negative
  W(0, 1) = -1;
  EXPECT_THROW(gaussian_dlm_log(y, FF, GG, V, W), std::domain_error);
  // wrong size
  Matrix<double, Dynamic, Dynamic> W1 = MatrixXd::Identity(3, 3);
  EXPECT_THROW(gaussian_dlm_log(y, FF, GG, V, W1), std::domain_error);
  // not square
  Matrix<double, Dynamic, Dynamic> W2 = MatrixXd::Identity(2, 3);
  EXPECT_THROW(gaussian_dlm_log(y, FF, GG, V, W2), std::domain_error);

}
<|endoftext|>


<commit_before>#include "circuits/element.h"

#include "matrix/matrix.h"
#include <sstream>
#include <cstdlib>

//TODO: not to use cout inside Element class
#include <iostream>
//

Element::Element()
{
}

Element::Element(string netlistLine, int &numElements,  int &nv, vector<string> &list)
{
    stringstream sstream(netlistLine);
    char na[MAX_NOME], nb[MAX_NOME], nc[MAX_NOME], nd[MAX_NOME];

    type = toupper( netlistLine[0] );
    sstream >> name;
    sstream.str( string(netlistLine, name.size(), string::npos) );
    if (type=='R' || type=='I' || type=='V') {
        sstream >> na >> nb >> valor;
        cout << name << " " << na << " " << nb << " " << valor << endl;
        a = number(na, nv, list);
        b = number(nb, nv, list);
    }
    else if (type=='G' || type=='E' || type=='F' || type=='H') {
        sstream >> na >> nb >> nc >> nd >> valor;
        cout << name << " " << na << " " << nb << " " << nc << " "
             << nd << " "<< valor << endl;
        a = number(na, nv, list);
        b = number(nb, nv, list);
        c = number(nc, nv, list);
        d = number(nd, nv, list);
    }
    else if (type=='O') {
        sstream >> na >> nb >> nc >> nd;
        cout << name << " " << na << " " << nb << " " << nc << " " << nd << " " << endl;
        a = number(na, nv, list);
        b = number(nb, nv, list);
        c = number(nc, nv, list);
        d = number(nd, nv, list);
    }
    else if (type=='*') { /* Comentario comeca com "*" */
        cout << "Comentario: " << netlistLine;
        numElements--;
    }
    else {
        cout << "Elemento desconhecido: " << netlistLine << endl;
        #if defined (WIN32) || defined(_WIN32)
        cin.get();
        #endif
        exit(EXIT_FAILURE);
    }
}

Element::Element(string name, double value, int a, int b, int c, int d, int x, int y)
{
    setType(name[0]);
    this->name = name;
    this->valor = value;
    this->a = a;
    this->b = b;
    this->c = c;
    this->d = d;
    this->x = x;
    this->y = y;
}

/** Rotina que conta os nos e atribui Element::numbers a eles */
int Element::number(const char *name, int &nv, vector<string> &list)
{
    int i=0, achou=0;

    while (!achou && i<=nv)
        if (!(achou=!list[i].compare(name))) i++;
    if (!achou) {
        if (nv==MAX_NOS) {
            cout << "O programa so aceita ate " << nv <<  " nos" << endl;
            #if defined (WIN32) || defined(_WIN32)
            cin.get();
            #endif
            exit(EXIT_FAILURE);
        }
        nv++;
        list[nv] = name;
        return nv; /* novo no */
    }
    else {
        return i; /* no ja conhecido */
    }
}
char Element::getType() const
{
    return type;
}

void Element::setType(char value)
{
    type = value;
}

<commit_msg>Uses setter in Element constructor<commit_after>#include "circuits/element.h"

#include "matrix/matrix.h"
#include <sstream>
#include <cstdlib>

//TODO: not to use cout inside Element class
#include <iostream>
//

Element::Element()
{
}

Element::Element(string netlistLine, int &numElements,  int &nv, vector<string> &list)
{
    stringstream sstream(netlistLine);
    char na[MAX_NOME], nb[MAX_NOME], nc[MAX_NOME], nd[MAX_NOME];

    sstream >> name;
    setType(name[0]);
    sstream.str( string(netlistLine, name.size(), string::npos) );
    if (type=='R' || type=='I' || type=='V') {
        sstream >> na >> nb >> valor;
        cout << name << " " << na << " " << nb << " " << valor << endl;
        a = number(na, nv, list);
        b = number(nb, nv, list);
    }
    else if (type=='G' || type=='E' || type=='F' || type=='H') {
        sstream >> na >> nb >> nc >> nd >> valor;
        cout << name << " " << na << " " << nb << " " << nc << " "
             << nd << " "<< valor << endl;
        a = number(na, nv, list);
        b = number(nb, nv, list);
        c = number(nc, nv, list);
        d = number(nd, nv, list);
    }
    else if (type=='O') {
        sstream >> na >> nb >> nc >> nd;
        cout << name << " " << na << " " << nb << " " << nc << " " << nd << " " << endl;
        a = number(na, nv, list);
        b = number(nb, nv, list);
        c = number(nc, nv, list);
        d = number(nd, nv, list);
    }
    else if (type=='*') { /* Comentario comeca com "*" */
        cout << "Comentario: " << netlistLine;
        numElements--;
    }
    else {
        cout << "Elemento desconhecido: " << netlistLine << endl;
        #if defined (WIN32) || defined(_WIN32)
        cin.get();
        #endif
        exit(EXIT_FAILURE);
    }
}

Element::Element(string name, double value, int a, int b, int c, int d, int x, int y)
{
    setType(name[0]);
    this->name = name;
    this->valor = value;
    this->a = a;
    this->b = b;
    this->c = c;
    this->d = d;
    this->x = x;
    this->y = y;
}

/** Rotina que conta os nos e atribui Element::numbers a eles */
int Element::number(const char *name, int &nv, vector<string> &list)
{
    int i=0, achou=0;

    while (!achou && i<=nv)
        if (!(achou=!list[i].compare(name))) i++;
    if (!achou) {
        if (nv==MAX_NOS) {
            cout << "O programa so aceita ate " << nv <<  " nos" << endl;
            #if defined (WIN32) || defined(_WIN32)
            cin.get();
            #endif
            exit(EXIT_FAILURE);
        }
        nv++;
        list[nv] = name;
        return nv; /* novo no */
    }
    else {
        return i; /* no ja conhecido */
    }
}

char Element::getType() const
{
    return type;
}

void Element::setType(char value)
{
    type = toupper(value);
}

<|endoftext|>


<commit_before><commit_msg>layers: Intercept PushDescriptorSets in CV<commit_after><|endoftext|>


<commit_before>#include <algorithm>

// not used but prevents compilation errors with cuda 7 RC
#include <thrust/transform.h>

#include "chrono_parallel/collision/ChCAABBGenerator.h"
using namespace chrono::collision;

__constant__ uint numAABB_const;

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
static __device__ __host__ void ComputeAABBSphere(const real& radius, const real3& position, real3& minp, real3& maxp) {
  minp = position - R3(radius);
  maxp = position + R3(radius);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
static __device__ __host__ void ComputeAABBTriangle(const real3& A,
                                                    const real3& B,
                                                    const real3& C,
                                                    real3& minp,
                                                    real3& maxp) {
  minp.x = std::min(A.x, std::min(B.x, C.x));
  minp.y = std::min(A.y, std::min(B.y, C.y));
  minp.z = std::min(A.z, std::min(B.z, C.z));
  maxp.x = std::max(A.x, std::max(B.x, C.x));
  maxp.y = std::max(A.y, std::max(B.y, C.y));
  maxp.z = std::max(A.z, std::max(B.z, C.z));
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
static void ComputeAABBBox(const real3& dim,
                           const real3& lpositon,
                           const real3& positon,
                           const real4& lrotation,
                           const real4& rotation,
                           real3& minp,
                           real3& maxp) {
  real4 q1 = mult(rotation, lrotation);
  M33 rotmat = AMat(q1);
  rotmat = AbsMat(rotmat);

  real3 temp = MatMult(rotmat, dim);

  real3 pos = quatRotate(lpositon, rotation) + positon;
  minp = pos - temp;
  maxp = pos + temp;

  // cout<<minp.x<<" "<<minp.y<<" "<<minp.z<<"  |  "<<maxp.x<<" "<<maxp.y<<" "<<maxp.z<<endl;
  //    real3 pos = quatRotate(lpositon, rotation) + positon; //new position
  //    real4 q1 = mult(rotation, lrotation); //full rotation
  //    real4 q = R4(q1.y, q1.z, q1.w, q1.x);
  //    real t[3] = { pos.x, pos.y, pos.z };
  //    real mina[3] = { -dim.x, -dim.y, -dim.z };
  //    real maxa[3] = { dim.x, dim.y, dim.z };
  //    real minb[3] = { 0, 0, 0 };
  //    real maxb[3] = { 0, 0, 0 };
  //    real m[3][3];
  //    real qx2 = q.x * q.x;
  //    real qy2 = q.y * q.y;
  //    real qz2 = q.z * q.z;
  //    m[0][0] = 1 - 2 * qy2 - 2 * qz2;
  //    m[1][0] = 2 * q.x * q.y + 2 * q.z * q.w;
  //    m[2][0] = 2 * q.x * q.z - 2 * q.y * q.w;
  //    m[0][1] = 2 * q.x * q.y - 2 * q.z * q.w;
  //    m[1][1] = 1 - 2 * qx2 - 2 * qz2;
  //    m[2][1] = 2 * q.y * q.z + 2 * q.x * q.w   ;
  //    m[0][2] = 2 * q.x * q.z + 2 * q.y * q.w;
  //    m[1][2] = 2 * q.y * q.z - 2 * q.x * q.w;
  //    m[2][2] = 1 - 2 * qx2 - 2 * qy2;
  //
  //    // For all three axes
  //    for (int i = 0; i < 3; i++) {
  //        // Start by adding in translation
  //        minb[i] = maxb[i] = t[i];
  //
  //        // Form extent by summing smaller and larger terms respectively
  //        for (int j = 0; j < 3; j++) {
  //            real e = m[i][j] * mina[j];
  //            real f = m[i][j] * maxa[j];
  //
  //            if (e < f) {
  //                minb[i] += e;
  //                maxb[i] += f;
  //            } else {
  //                minb[i] += f;
  //                maxb[i] += e;
  //            }
  //        }
  //    }

  //    minp = R3(minb[0], minb[1], minb[2]);
  //    maxp = R3(maxb[0], maxb[1], maxb[2]);
}

static void ComputeAABBCone(const real3& dim,
                            const real3& lpositon,
                            const real3& positon,
                            const real4& lrotation,
                            const real4& rotation,
                            real3& minp,
                            real3& maxp) {
  real4 q1 = mult(rotation, lrotation);
  M33 rotmat = AMat(q1);
  rotmat = AbsMat(rotmat);

  real3 temp = MatMult(rotmat, R3(dim.x, dim.y, dim.z / 2.0));

  real3 pos = quatRotate(lpositon - R3(0, 0, dim.z / 2.0), rotation) + positon;
  minp = pos - temp;
  maxp = pos + temp;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
static void ComputeAABBConvex(const real3* convex_points,
                              const real3& B,
                              const real3& lpos,
                              const real3& pos,
                              const real4& rot,
                              real3& minp,
                              real3& maxp) {
  int start = B.y;
  int size = B.x;
  real3 point_0 = quatRotate(convex_points[start] + lpos, rot) + pos;

  minp = maxp = point_0;
  for (int i = start; i < start + size; i++) {
    real3 p = quatRotate(convex_points[i] + lpos, rot) + pos;
    if (minp.x > p.x) {
      minp.x = p.x;
    }
    if (minp.y > p.y) {
      minp.y = p.y;
    }
    if (minp.z > p.z) {
      minp.z = p.z;
    }
    if (maxp.x < p.x) {
      maxp.x = p.x;
    }
    if (maxp.y < p.y) {
      maxp.y = p.y;
    }
    if (maxp.z < p.z) {
      maxp.z = p.z;
    }
  }

  minp = minp - R3(B.z);
  maxp = maxp + R3(B.z);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
__device__ __host__ void function_ComputeAABB(const uint& index,
                                              const shape_type* obj_data_T,
                                              const real3* obj_data_A,
                                              const real3* obj_data_B,
                                              const real3* obj_data_C,
                                              const real4* obj_data_R,
                                              const uint* obj_data_ID,
                                              const real3* convex_points,
                                              const real3* body_pos,
                                              const real4* body_rot,
                                              const uint& numAABB,
                                              const real& collision_envelope,
                                              real3* aabb_data) {
  shape_type type = obj_data_T[index];
  uint id = obj_data_ID[index];
  real3 A = obj_data_A[index];
  real3 B = obj_data_B[index];
  real3 C = obj_data_C[index];
  real3 position = body_pos[id];
  real4 rotation = (mult(body_rot[id], obj_data_R[index]));
  real3 temp_min;
  real3 temp_max;

  if (type == SPHERE) {
    A = quatRotate(A, body_rot[id]);
    ComputeAABBSphere(B.x + collision_envelope, A + position, temp_min, temp_max);
    // if(id==6){
    // cout<<temp_min.x<<" "<<temp_min.y<<" "<<temp_min.z<<"  |  "<<temp_max.x<<" "<<temp_max.y<<" "<<temp_max.z<<endl;

    //}
  } else if (type == TRIANGLEMESH) {
    A = quatRotate(A, body_rot[id]) + position;
    B = quatRotate(B, body_rot[id]) + position;
    C = quatRotate(C, body_rot[id]) + position;
    ComputeAABBTriangle(A, B, C, temp_min, temp_max);
  } else if (type == ELLIPSOID || type == BOX || type == CYLINDER || type == CONE) {
    ComputeAABBBox(B + collision_envelope, A, position, obj_data_R[index], body_rot[id], temp_min, temp_max);
  } else if (type == ROUNDEDBOX || type == ROUNDEDCYL || type == ROUNDEDCONE) {
    ComputeAABBBox(B + C.x + collision_envelope, A, position, obj_data_R[index], body_rot[id], temp_min, temp_max);
  } else if (type == CAPSULE) {
    real3 B_ = R3(B.x, B.x + B.y, B.z) + collision_envelope;
    ComputeAABBBox(B_, A, position, obj_data_R[index], body_rot[id], temp_min, temp_max);
  } else if (type == CONVEX) {
    ComputeAABBConvex(convex_points, B, A, position, rotation, temp_min, temp_max);
    temp_min -= collision_envelope;
    temp_max += collision_envelope;
  } else {
    return;
  }

  aabb_data[index] = temp_min;
  aabb_data[index + numAABB] = temp_max;
}

void ChCAABBGenerator::host_ComputeAABB(const shape_type* obj_data_T,
                                        const real3* obj_data_A,
                                        const real3* obj_data_B,
                                        const real3* obj_data_C,
                                        const real4* obj_data_R,
                                        const uint* obj_data_ID,
                                        const real3* convex_data,
                                        const real3* body_pos,
                                        const real4* body_rot,
                                        const real collision_envelope,
                                        real3* aabb_data) {
#pragma omp parallel for

  for (int i = 0; i < numAABB; i++) {
    function_ComputeAABB(i, obj_data_T, obj_data_A, obj_data_B, obj_data_C, obj_data_R, obj_data_ID, convex_data,
                         body_pos, body_rot, numAABB, collision_envelope, aabb_data);
  }
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ChCAABBGenerator::ChCAABBGenerator() {
}

void ChCAABBGenerator::GenerateAABB(const custom_vector<shape_type>& obj_data_T,
                                    const custom_vector<real3>& obj_data_A,
                                    const custom_vector<real3>& obj_data_B,
                                    const custom_vector<real3>& obj_data_C,
                                    const custom_vector<real4>& obj_data_R,
                                    const custom_vector<uint>& obj_data_ID,
                                    const custom_vector<real3>& convex_data,
                                    const custom_vector<real3>& body_pos,
                                    const custom_vector<real4>& body_rot,
                                    const real collision_envelope,
                                    custom_vector<real3>& aabb_data) {
#if PRINT_LEVEL == 2
  cout << "AABB START" << endl;
#endif

  numAABB = obj_data_T.size();
  aabb_data.resize(numAABB * 2);
#ifdef SIM_ENABLE_GPU_MODE
  COPY_TO_CONST_MEM(numAABB);
  device_ComputeAABB __KERNEL__(BLOCKS(numAABB), THREADS)(
      CASTS(obj_data_T.data()), CASTR3(obj_data_A.data()), CASTR3(obj_data_B.data()), CASTR3(obj_data_C.data()),
      CASTR4(obj_data_R.data()), CASTU1(obj_data_ID.data()), CASTR3(body_pos.data()), CASTR4(body_rot.data()),
      CASTR3(aabb_data.data()));
#else
  host_ComputeAABB(obj_data_T.data(), obj_data_A.data(), obj_data_B.data(), obj_data_C.data(), obj_data_R.data(),
                   obj_data_ID.data(), convex_data.data(), body_pos.data(), body_rot.data(), collision_envelope,
                   aabb_data.data());
#endif
#if PRINT_LEVEL == 2
  cout << "AABB END" << endl;
#endif
#if PRINT_LEVEL == 2
//    for(int i=0; i<numAABB; i++){
//
//    	cout<<real3(aabb_data[i]).x<<" "<<real3(aabb_data[i]).y<<" "<<real3(aabb_data[i]).z<<endl;
//    	cout<<real3(aabb_data[i+numAABB]).x<<" "<<real3(aabb_data[i+numAABB]).y<<"
//    "<<real3(aabb_data[i+numAABB]).z<<endl;
//    	cout<<"-------"<<endl;
//    }

#endif
}
<commit_msg>Use log(trace) instead of print level<commit_after>#include <algorithm>

// not used but prevents compilation errors with cuda 7 RC
#include <thrust/transform.h>

#include "chrono_parallel/collision/ChCAABBGenerator.h"
using namespace chrono::collision;

__constant__ uint numAABB_const;

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
static __device__ __host__ void ComputeAABBSphere(const real& radius, const real3& position, real3& minp, real3& maxp) {
  minp = position - R3(radius);
  maxp = position + R3(radius);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
static __device__ __host__ void ComputeAABBTriangle(const real3& A,
                                                    const real3& B,
                                                    const real3& C,
                                                    real3& minp,
                                                    real3& maxp) {
  minp.x = std::min(A.x, std::min(B.x, C.x));
  minp.y = std::min(A.y, std::min(B.y, C.y));
  minp.z = std::min(A.z, std::min(B.z, C.z));
  maxp.x = std::max(A.x, std::max(B.x, C.x));
  maxp.y = std::max(A.y, std::max(B.y, C.y));
  maxp.z = std::max(A.z, std::max(B.z, C.z));
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
static void ComputeAABBBox(const real3& dim,
                           const real3& lpositon,
                           const real3& positon,
                           const real4& lrotation,
                           const real4& rotation,
                           real3& minp,
                           real3& maxp) {
  real4 q1 = mult(rotation, lrotation);
  M33 rotmat = AMat(q1);
  rotmat = AbsMat(rotmat);

  real3 temp = MatMult(rotmat, dim);

  real3 pos = quatRotate(lpositon, rotation) + positon;
  minp = pos - temp;
  maxp = pos + temp;

  // cout<<minp.x<<" "<<minp.y<<" "<<minp.z<<"  |  "<<maxp.x<<" "<<maxp.y<<" "<<maxp.z<<endl;
  //    real3 pos = quatRotate(lpositon, rotation) + positon; //new position
  //    real4 q1 = mult(rotation, lrotation); //full rotation
  //    real4 q = R4(q1.y, q1.z, q1.w, q1.x);
  //    real t[3] = { pos.x, pos.y, pos.z };
  //    real mina[3] = { -dim.x, -dim.y, -dim.z };
  //    real maxa[3] = { dim.x, dim.y, dim.z };
  //    real minb[3] = { 0, 0, 0 };
  //    real maxb[3] = { 0, 0, 0 };
  //    real m[3][3];
  //    real qx2 = q.x * q.x;
  //    real qy2 = q.y * q.y;
  //    real qz2 = q.z * q.z;
  //    m[0][0] = 1 - 2 * qy2 - 2 * qz2;
  //    m[1][0] = 2 * q.x * q.y + 2 * q.z * q.w;
  //    m[2][0] = 2 * q.x * q.z - 2 * q.y * q.w;
  //    m[0][1] = 2 * q.x * q.y - 2 * q.z * q.w;
  //    m[1][1] = 1 - 2 * qx2 - 2 * qz2;
  //    m[2][1] = 2 * q.y * q.z + 2 * q.x * q.w   ;
  //    m[0][2] = 2 * q.x * q.z + 2 * q.y * q.w;
  //    m[1][2] = 2 * q.y * q.z - 2 * q.x * q.w;
  //    m[2][2] = 1 - 2 * qx2 - 2 * qy2;
  //
  //    // For all three axes
  //    for (int i = 0; i < 3; i++) {
  //        // Start by adding in translation
  //        minb[i] = maxb[i] = t[i];
  //
  //        // Form extent by summing smaller and larger terms respectively
  //        for (int j = 0; j < 3; j++) {
  //            real e = m[i][j] * mina[j];
  //            real f = m[i][j] * maxa[j];
  //
  //            if (e < f) {
  //                minb[i] += e;
  //                maxb[i] += f;
  //            } else {
  //                minb[i] += f;
  //                maxb[i] += e;
  //            }
  //        }
  //    }

  //    minp = R3(minb[0], minb[1], minb[2]);
  //    maxp = R3(maxb[0], maxb[1], maxb[2]);
}

static void ComputeAABBCone(const real3& dim,
                            const real3& lpositon,
                            const real3& positon,
                            const real4& lrotation,
                            const real4& rotation,
                            real3& minp,
                            real3& maxp) {
  real4 q1 = mult(rotation, lrotation);
  M33 rotmat = AMat(q1);
  rotmat = AbsMat(rotmat);

  real3 temp = MatMult(rotmat, R3(dim.x, dim.y, dim.z / 2.0));

  real3 pos = quatRotate(lpositon - R3(0, 0, dim.z / 2.0), rotation) + positon;
  minp = pos - temp;
  maxp = pos + temp;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
static void ComputeAABBConvex(const real3* convex_points,
                              const real3& B,
                              const real3& lpos,
                              const real3& pos,
                              const real4& rot,
                              real3& minp,
                              real3& maxp) {
  int start = B.y;
  int size = B.x;
  real3 point_0 = quatRotate(convex_points[start] + lpos, rot) + pos;

  minp = maxp = point_0;
  for (int i = start; i < start + size; i++) {
    real3 p = quatRotate(convex_points[i] + lpos, rot) + pos;
    if (minp.x > p.x) {
      minp.x = p.x;
    }
    if (minp.y > p.y) {
      minp.y = p.y;
    }
    if (minp.z > p.z) {
      minp.z = p.z;
    }
    if (maxp.x < p.x) {
      maxp.x = p.x;
    }
    if (maxp.y < p.y) {
      maxp.y = p.y;
    }
    if (maxp.z < p.z) {
      maxp.z = p.z;
    }
  }

  minp = minp - R3(B.z);
  maxp = maxp + R3(B.z);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
__device__ __host__ void function_ComputeAABB(const uint& index,
                                              const shape_type* obj_data_T,
                                              const real3* obj_data_A,
                                              const real3* obj_data_B,
                                              const real3* obj_data_C,
                                              const real4* obj_data_R,
                                              const uint* obj_data_ID,
                                              const real3* convex_points,
                                              const real3* body_pos,
                                              const real4* body_rot,
                                              const uint& numAABB,
                                              const real& collision_envelope,
                                              real3* aabb_data) {
  shape_type type = obj_data_T[index];
  uint id = obj_data_ID[index];
  real3 A = obj_data_A[index];
  real3 B = obj_data_B[index];
  real3 C = obj_data_C[index];
  real3 position = body_pos[id];
  real4 rotation = (mult(body_rot[id], obj_data_R[index]));
  real3 temp_min;
  real3 temp_max;

  if (type == SPHERE) {
    A = quatRotate(A, body_rot[id]);
    ComputeAABBSphere(B.x + collision_envelope, A + position, temp_min, temp_max);
    // if(id==6){
    // cout<<temp_min.x<<" "<<temp_min.y<<" "<<temp_min.z<<"  |  "<<temp_max.x<<" "<<temp_max.y<<" "<<temp_max.z<<endl;

    //}
  } else if (type == TRIANGLEMESH) {
    A = quatRotate(A, body_rot[id]) + position;
    B = quatRotate(B, body_rot[id]) + position;
    C = quatRotate(C, body_rot[id]) + position;
    ComputeAABBTriangle(A, B, C, temp_min, temp_max);
  } else if (type == ELLIPSOID || type == BOX || type == CYLINDER || type == CONE) {
    ComputeAABBBox(B + collision_envelope, A, position, obj_data_R[index], body_rot[id], temp_min, temp_max);
  } else if (type == ROUNDEDBOX || type == ROUNDEDCYL || type == ROUNDEDCONE) {
    ComputeAABBBox(B + C.x + collision_envelope, A, position, obj_data_R[index], body_rot[id], temp_min, temp_max);
  } else if (type == CAPSULE) {
    real3 B_ = R3(B.x, B.x + B.y, B.z) + collision_envelope;
    ComputeAABBBox(B_, A, position, obj_data_R[index], body_rot[id], temp_min, temp_max);
  } else if (type == CONVEX) {
    ComputeAABBConvex(convex_points, B, A, position, rotation, temp_min, temp_max);
    temp_min -= collision_envelope;
    temp_max += collision_envelope;
  } else {
    return;
  }

  aabb_data[index] = temp_min;
  aabb_data[index + numAABB] = temp_max;
}

void ChCAABBGenerator::host_ComputeAABB(const shape_type* obj_data_T,
                                        const real3* obj_data_A,
                                        const real3* obj_data_B,
                                        const real3* obj_data_C,
                                        const real4* obj_data_R,
                                        const uint* obj_data_ID,
                                        const real3* convex_data,
                                        const real3* body_pos,
                                        const real4* body_rot,
                                        const real collision_envelope,
                                        real3* aabb_data) {
#pragma omp parallel for

  for (int i = 0; i < numAABB; i++) {
    function_ComputeAABB(i, obj_data_T, obj_data_A, obj_data_B, obj_data_C, obj_data_R, obj_data_ID, convex_data,
                         body_pos, body_rot, numAABB, collision_envelope, aabb_data);
  }
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ChCAABBGenerator::ChCAABBGenerator() {
}

void ChCAABBGenerator::GenerateAABB(const custom_vector<shape_type>& obj_data_T,
                                    const custom_vector<real3>& obj_data_A,
                                    const custom_vector<real3>& obj_data_B,
                                    const custom_vector<real3>& obj_data_C,
                                    const custom_vector<real4>& obj_data_R,
                                    const custom_vector<uint>& obj_data_ID,
                                    const custom_vector<real3>& convex_data,
                                    const custom_vector<real3>& body_pos,
                                    const custom_vector<real4>& body_rot,
                                    const real collision_envelope,
                                    custom_vector<real3>& aabb_data) {

  LOG(TRACE) << "AABB START";

  numAABB = obj_data_T.size();
  aabb_data.resize(numAABB * 2);
#ifdef SIM_ENABLE_GPU_MODE
  COPY_TO_CONST_MEM(numAABB);
  device_ComputeAABB __KERNEL__(BLOCKS(numAABB), THREADS)(
      CASTS(obj_data_T.data()), CASTR3(obj_data_A.data()), CASTR3(obj_data_B.data()), CASTR3(obj_data_C.data()),
      CASTR4(obj_data_R.data()), CASTU1(obj_data_ID.data()), CASTR3(body_pos.data()), CASTR4(body_rot.data()),
      CASTR3(aabb_data.data()));
#else
  host_ComputeAABB(obj_data_T.data(), obj_data_A.data(), obj_data_B.data(), obj_data_C.data(), obj_data_R.data(),
                   obj_data_ID.data(), convex_data.data(), body_pos.data(), body_rot.data(), collision_envelope,
                   aabb_data.data());
#endif

  LOG(TRACE) << "AABB END";

#if PRINT_LEVEL == 2
//    for(int i=0; i<numAABB; i++){
//
//    	cout<<real3(aabb_data[i]).x<<" "<<real3(aabb_data[i]).y<<" "<<real3(aabb_data[i]).z<<endl;
//    	cout<<real3(aabb_data[i+numAABB]).x<<" "<<real3(aabb_data[i+numAABB]).y<<"
//    "<<real3(aabb_data[i+numAABB]).z<<endl;
//    	cout<<"-------"<<endl;
//    }

#endif
}
<|endoftext|>


<commit_before>//
// Number Theory
//
#define IsComp(n)  (_c[n>>6]&(1<<((n>>1)&31)))
#define SetComp(n) _c[n>>6]|=(1<<((n>>1)&31))
const int MAXP	 = 46341;  // sqrt(2^31)
const int SQRP	 =	 216;  // sqrt(MAX)
const int MAX_NP =	5435;  // 1.26 * MAXP/log(MAXP)
int _c[(MAXP>>6)+1];
int primes[MAX_NP];
int nprimes;
void prime_sieve() {
	for (int i = 3; i <= SQRP; i += 2)
		if (!IsComp(i)) for (int j = i*i; j <= MAXP; j+=i+i) SetComp(j);
	primes[nprimes++] = 2;
	for (int i=3; i <= MAXP; i+=2) if (!IsComp(i)) primes[nprimes++] = i;
}
bool is_prime(int n) {
	if (n < 2 || (n > 2 && n % 2 == 0)) return false;
	return ! IsComp(n);
}

// Finds prime numbers between a and b, using basic primes up to sqrt(b)
// pre cond: a > 2 (handle that separately)
void prime_seg_sieve(i64 a, i64 b, IV &seg_primes) {
	BV pmap(b - a + 1, true);
	i64 sqr_b = sqrt(b);
	For (IV, pp, primes) {
		int p = *pp;
		if (p > sqr_b) break;
		for (i64 j = (a+p-1)/p, v=(j==1?p+p:j*p); v <= b; v += p)
			pmap[v-a] = false;
	}
	seg_primes.clear();
	for (int i = a%2 ? 0 : 1, I = b - a + 1; i < I; i += 2)
		if (pmap[i])
			seg_primes.push_back(a + i);
}

void prime_factorize(int n, IIV &f) {
	int sn = sqrt(n);
	For (IV, p, primes) {
		int prime = *p;
		if (prime > sn) break; if (n % prime) continue;
		int e = 0; for (; n % prime == 0; e++, n /= prime);
		f.push_back(II(prime, e));
		sn = sqrt(n);
	}
	if (n > 1) f.push_back(II(n, 1));
}

template <typename T>
void divisors(T n, T *ds, int &s)
{
	s = 0;
	ds[s++] = 1;
	int sn = sqrt(n);
	for (int i = 0; i < nprimes; ++i) {
		int &p = primes[i];
		if (p > sn) break;
		if (n % p != 0) continue;
		T q = 1; int t = s;
		while (n % p == 0) {
			q *= p, n /= p;
			for (int j = 0; j < t; ++j) ds[s++] = ds[j] * q;
		}
		sn = sqrt(n);
	}
	if (n > 1) {
		int t = s;
		for (int j = 0; j < t; ++j) ds[s++] = ds[j] * n;
	}
}

void euler_phi(int a[], int N) {
	for (int i = 1; i <= N; i++) a[i] = i;
	for (int i = 2; i <= N; i += 2) a[i] = i/2;
	for (int i = 3; i <= N; i += 2)
		if (a[i] == i) {
			--a[i]; for (int j = i+i; j <= N; j += i) a[j] -= a[j]/i; }
}

template <typename TB, typename TE>
TB mod_pow(TB b, TE e, TB m) {
	TB res = 1; for (; e; e >>= 1, b = b*b%m) if (e & 1) res = res*b%m;
	return res;
}

template<typename TB, typename TE>
TB sq_pow(TB b, TE e) {
	TB res = 1; for (; e; e >>= 1, b *= b) if (e & 1) res *= b;
	return res;
}

template<typename TB, typename TE>
void sq_pow(TB b, TE e, TB &r) {
	for (; e; e >>= 1, b *= b) if (e & 1) r *= b;
}

template<typename T>
T gcd(T a, T b) { for (T c = a%b; c != 0; a=b,b=c,c=a%b); return b; }

template<typename T>
void ext_euclid(T a, T b, T &x, T &y, T &g) {
	x = 0; y = 1; g = b;
	T m, n, q, r;
	for (T u=1, v=0; a != 0; g=a, a=r) {
		q = g / a; r = g % a;
		m = x-u*q; n = y-v*q;
		x=u; y=v; u=m; v=n;
	}
}

template<typename T>
T mod_inv(T n, T m) {
	T x, y, gcd;
	ext_euclid(n, m, x, y, gcd);
	if (gcd == 1) return x % m;
	return 0;
}

template <typename T>
T chinese_rem(int k, T *ns, T *as)
{
	T N = 1, x = 0, r, s, g;
	for (int i = 0; i < k; ++i) N *= ns[i];
	for (int i = 0; i < k; ++i) {
		ext_euclid(ns[i], N/ns[i], r, s, g);
		x += as[i]*s*(N/ns[i]);
		x %= N;
	}
	if (x < 0) x += N;
	return x;
}

// Calculates the highest exponent of prime p that divides n!
int pow_div_fact(int n, int p) {
	int sd = 0; for (int N=n; N; N /= p) sd += N % p; return (n-sd)/(p-1);
}

// Binomial coefficients
#define BCSIZE 1000
int bc[BCSIZE + 1][BCSIZE + 1];
void choose_table() {
	for (int n = 1; n <= BCSIZE; n++) { bc[n][n] = 1; bc[n][1] = n; }
	for (int n = 3; n <= BCSIZE; n++)
		for (int k = 2; k < n; k++)
			bc[n][k] = (bc[n-1][k-1] + bc[n-1][k]) % MOD;
}

int choose(int n, int k) {
	if (k > n) return 0;
	i64 r=1;
	for (int d = 1; d <= k; d++) {
		r *= n--;
		r /= d;
	}
	return r;
}
<commit_msg>Minor edit to choose() function<commit_after>//
// Number Theory
//
#define IsComp(n)  (_c[n>>6]&(1<<((n>>1)&31)))
#define SetComp(n) _c[n>>6]|=(1<<((n>>1)&31))
const int MAXP	 = 46341;  // sqrt(2^31)
const int SQRP	 =	 216;  // sqrt(MAX)
const int MAX_NP =	5435;  // 1.26 * MAXP/log(MAXP)
int _c[(MAXP>>6)+1];
int primes[MAX_NP];
int nprimes;
void prime_sieve() {
	for (int i = 3; i <= SQRP; i += 2)
		if (!IsComp(i)) for (int j = i*i; j <= MAXP; j+=i+i) SetComp(j);
	primes[nprimes++] = 2;
	for (int i=3; i <= MAXP; i+=2) if (!IsComp(i)) primes[nprimes++] = i;
}
bool is_prime(int n) {
	if (n < 2 || (n > 2 && n % 2 == 0)) return false;
	return ! IsComp(n);
}

// Finds prime numbers between a and b, using basic primes up to sqrt(b)
// pre cond: a > 2 (handle that separately)
void prime_seg_sieve(i64 a, i64 b, IV &seg_primes) {
	BV pmap(b - a + 1, true);
	i64 sqr_b = sqrt(b);
	For (IV, pp, primes) {
		int p = *pp;
		if (p > sqr_b) break;
		for (i64 j = (a+p-1)/p, v=(j==1?p+p:j*p); v <= b; v += p)
			pmap[v-a] = false;
	}
	seg_primes.clear();
	for (int i = a%2 ? 0 : 1, I = b - a + 1; i < I; i += 2)
		if (pmap[i])
			seg_primes.push_back(a + i);
}

void prime_factorize(int n, IIV &f) {
	int sn = sqrt(n);
	For (IV, p, primes) {
		int prime = *p;
		if (prime > sn) break; if (n % prime) continue;
		int e = 0; for (; n % prime == 0; e++, n /= prime);
		f.push_back(II(prime, e));
		sn = sqrt(n);
	}
	if (n > 1) f.push_back(II(n, 1));
}

template <typename T>
void divisors(T n, T *ds, int &s)
{
	s = 0;
	ds[s++] = 1;
	int sn = sqrt(n);
	for (int i = 0; i < nprimes; ++i) {
		int &p = primes[i];
		if (p > sn) break;
		if (n % p != 0) continue;
		T q = 1; int t = s;
		while (n % p == 0) {
			q *= p, n /= p;
			for (int j = 0; j < t; ++j) ds[s++] = ds[j] * q;
		}
		sn = sqrt(n);
	}
	if (n > 1) {
		int t = s;
		for (int j = 0; j < t; ++j) ds[s++] = ds[j] * n;
	}
}

void euler_phi(int a[], int N) {
	for (int i = 1; i <= N; i++) a[i] = i;
	for (int i = 2; i <= N; i += 2) a[i] = i/2;
	for (int i = 3; i <= N; i += 2)
		if (a[i] == i) {
			--a[i]; for (int j = i+i; j <= N; j += i) a[j] -= a[j]/i; }
}

template <typename TB, typename TE>
TB mod_pow(TB b, TE e, TB m) {
	TB res = 1; for (; e; e >>= 1, b = b*b%m) if (e & 1) res = res*b%m;
	return res;
}

template<typename TB, typename TE>
TB sq_pow(TB b, TE e) {
	TB res = 1; for (; e; e >>= 1, b *= b) if (e & 1) res *= b;
	return res;
}

template<typename TB, typename TE>
void sq_pow(TB b, TE e, TB &r) {
	for (; e; e >>= 1, b *= b) if (e & 1) r *= b;
}

template<typename T>
T gcd(T a, T b) { for (T c = a%b; c != 0; a=b,b=c,c=a%b); return b; }

template<typename T>
void ext_euclid(T a, T b, T &x, T &y, T &g) {
	x = 0; y = 1; g = b;
	T m, n, q, r;
	for (T u=1, v=0; a != 0; g=a, a=r) {
		q = g / a; r = g % a;
		m = x-u*q; n = y-v*q;
		x=u; y=v; u=m; v=n;
	}
}

template<typename T>
T mod_inv(T n, T m) {
	T x, y, gcd;
	ext_euclid(n, m, x, y, gcd);
	if (gcd == 1) return x % m;
	return 0;
}

template <typename T>
T chinese_rem(int k, T *ns, T *as)
{
	T N = 1, x = 0, r, s, g;
	for (int i = 0; i < k; ++i) N *= ns[i];
	for (int i = 0; i < k; ++i) {
		ext_euclid(ns[i], N/ns[i], r, s, g);
		x += as[i]*s*(N/ns[i]);
		x %= N;
	}
	if (x < 0) x += N;
	return x;
}

// Calculates the highest exponent of prime p that divides n!
int pow_div_fact(int n, int p) {
	int sd = 0; for (int N=n; N; N /= p) sd += N % p; return (n-sd)/(p-1);
}

// Binomial coefficients
#define BCSIZE 1000
int bc[BCSIZE + 1][BCSIZE + 1];
void choose_table() {
	for (int n = 1; n <= BCSIZE; n++) { bc[n][n] = 1; bc[n][1] = n; }
	for (int n = 3; n <= BCSIZE; n++)
		for (int k = 2; k < n; k++)
			bc[n][k] = (bc[n-1][k-1] + bc[n-1][k]) % MOD;
}

template <typename T>
T choose(T n, T k) {
	if (k > n) return 0;
	if (n-k < k) k = n-k;
	T r=1;
	for (T d = 1; d <= k; d++)
		r *= n--, r /= d;
	return r;
}
<|endoftext|>


<commit_before><commit_msg>Ignore metainstructions during the shrink wrap analysis<commit_after><|endoftext|>