<commit_before>/*
 * Copyright (c) 2017, Ubiquity Robotics
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. 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.
 *
 * 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.
 *
 * The views and conclusions contained in the software and documentation are
 * those of the authors and should not be interpreted as representing official
 * policies, either expressed or implied, of the FreeBSD Project.
 *
 */

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Path.h>

#include <actionlib/server/simple_action_server.h>
#include <move_base_msgs/MoveBaseAction.h>

#include <list>
#include <string>

typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;

class MoveBasic {
  private:
    ros::Subscriber goalSub;

    ros::Publisher goalPub;
    ros::Publisher cmdPub;
    ros::Publisher pathPub;

    std::unique_ptr<MoveBaseActionServer> actionServer;

    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener listener;

    double maxAngularVelocity;
    double minAngularVelocity;
    double angularAcceleration;
    double angularTolerance;

    double minLinearVelocity;
    double maxLinearVelocity;
    double linearAcceleration;
    double linearTolerance;

    tf2::Transform goalOdom;

    void goalCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);

    void executeAction(const move_base_msgs::MoveBaseGoalConstPtr& goal);

    void sendCmd(double angular, double linear);

    bool getTransform(const std::string& from, const std::string& to,
                      tf2::Transform& tf);

    bool handleRotation();
    bool handleLinear();

  public:
    MoveBasic();

    void run();

    bool moveLinear(double requestedDistance);
    bool rotateAbs(double requestedYaw);
    bool rotateRel(double yaw);
};

// Radians to degrees

static double rad2deg(double rad)
{
    return rad * 180.0 / M_PI;
}

// Adjust angle to be between -PI and PI

static void normalizeAngle(double& angle)
{
    if (angle < -M_PI) {
         angle += 2 * M_PI;
    }
    if (angle > M_PI) {
        angle -= 2 * M_PI;
    }
}

// retreive the 3 DOF we are interested in from a Transform

static void getPose(const tf2::Transform& tf, double& x, double& y, double& yaw)
{
    tf2::Vector3 trans = tf.getOrigin();
    x = trans.x();
    y = trans.y();

    double roll, pitch;
    tf.getBasis().getRPY(roll, pitch, yaw);
}

// Constructor

MoveBasic::MoveBasic(): tfBuffer(ros::Duration(30.0)),
                        listener(tfBuffer)
{
    ros::NodeHandle nh("~");

    nh.param<double>("min_angular_velocity", minAngularVelocity, 0.1);
    nh.param<double>("max_angular_velocity", maxAngularVelocity, 1.0);
    nh.param<double>("angular_acceleration", angularAcceleration, 0.3);
    nh.param<double>("angular_tolerance", angularTolerance, 0.01);

    nh.param<double>("min_linear_velocity", minLinearVelocity, 0.1);
    nh.param<double>("max_linear_velocity", maxLinearVelocity, 0.5);
    nh.param<double>("linear_acceleration", linearAcceleration, 0.5);
    nh.param<double>("linear_tolerance", linearTolerance, 0.01);

    cmdPub = ros::Publisher(nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1));

    pathPub = ros::Publisher(nh.advertise<nav_msgs::Path>("/plan", 1));

    goalSub = nh.subscribe("/move_base_simple/goal", 1,
                            &MoveBasic::goalCallback, this);

    ros::NodeHandle actionNh("");
    actionServer.reset(new MoveBaseActionServer(actionNh,
        "move_base", boost::bind(&MoveBasic::executeAction, this, _1), false));

    actionServer->start();
    goalPub = actionNh.advertise<move_base_msgs::MoveBaseActionGoal>(
      "/move_base/goal", 1);

    ROS_INFO("Move Basic ready");
}

// Lookup the specified transform, returns true on success

bool MoveBasic::getTransform(const std::string& from, const std::string& to,
                             tf2::Transform& tf)
{
    try {
        geometry_msgs::TransformStamped tfs =
            tfBuffer.lookupTransform(to, from, ros::Time(0));
        tf2::fromMsg(tfs.transform, tf);
        return true;
    }
    catch (tf2::TransformException &ex) {
         ROS_WARN("%s", ex.what());
         return false;
    }
}

// Called when a simple goal message is received

void MoveBasic::goalCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    ROS_INFO("Received simple goal");
    // send the goal to the action server
    move_base_msgs::MoveBaseActionGoal actionGoal;
    actionGoal.header.stamp = ros::Time::now();
    actionGoal.goal.target_pose = *msg;

    goalPub.publish(actionGoal);
}

// Called when an action goal is received
void MoveBasic::executeAction(const move_base_msgs::MoveBaseGoalConstPtr& msg)
{
    tf2::Transform goal;
    tf2::fromMsg(msg->target_pose.pose, goal);
    std::string frameId = msg->target_pose.header.frame_id;
    // Needed for RobotCommander
    if (frameId[0] == '/')
        frameId = frameId.substr(1);

    double x, y, yaw;
    getPose(goal, x, y, yaw);
    ROS_INFO("Received goal %f %f %f", x, y, rad2deg(yaw));

    tf2::Transform tfMapOdom;
    if (!getTransform(frameId, "odom", tfMapOdom)) {
        actionServer->setAborted(move_base_msgs::MoveBaseResult(),
                                 "Cannot determine robot pose");
        return;
    }
    goalOdom = tfMapOdom * goal;

    getPose(goalOdom, x, y, yaw);
    ROS_INFO("Goal in odom  %f %f %f", x, y, rad2deg(yaw));

    nav_msgs::Path path;
    geometry_msgs::PoseStamped p0, p1;
    path.header.frame_id = "odom";
    p0.pose.position.x = x;
    p0.pose.position.y = y;
    path.poses.push_back(p0);

    tf2::Transform poseOdom;
    if (!getTransform("base_link", "odom", poseOdom)) {
         actionServer->setAborted(move_base_msgs::MoveBaseResult(),
                                 "Cannot determine robot pose");
         return;
    }
    getPose(poseOdom, x, y, yaw);
    p1.pose.position.x = x;
    p1.pose.position.y = y;
    path.poses.push_back(p1);

    pathPub.publish(path);

    if (!handleRotation()) {
        actionServer->setAborted(move_base_msgs::MoveBaseResult(),
                                 "Rotation failed");
        return;
    }
    if (!handleLinear()) {
        actionServer->setAborted(move_base_msgs::MoveBaseResult(),
                                 "Linear movement failed");
        return;
    }
    getPose(goalOdom, x, y, yaw);
    rotateAbs(yaw);

    actionServer->setSucceeded();
}

// Send a motion command

void MoveBasic::sendCmd(double angular, double linear)
{
   geometry_msgs::Twist msg;
   msg.angular.z = angular;
   msg.linear.x = linear;

   cmdPub.publish(msg);
}

// Main loop

void MoveBasic::run()
{
    ros::Rate r(20);
    while (ros::ok()) {
        ros::spinOnce();
        r.sleep();
    }
}

// Do angular part of goal

bool MoveBasic::handleRotation()
{
    tf2::Transform poseOdom;
    if (!getTransform("base_link", "odom", poseOdom)) {
         ROS_WARN("Cannot determine robot pose for rotation");
         return false;
    }

    tf2::Vector3 linear = goalOdom.getOrigin() - poseOdom.getOrigin();
    // don't do initial rotation if there is no translation
    if (linear.length() == 0) {
        return true;
    }
    double requestedYaw = atan2(linear.y(), linear.x());

    if (requestedYaw == 0) {
        return true;
    }
    return rotateAbs(requestedYaw);
}

// Rotate relative to current orientation

bool MoveBasic::rotateRel(double yaw)
{
    tf2::Transform poseOdom;
    if (!getTransform("base_link", "odom", poseOdom)) {
         ROS_WARN("Cannot determine robot pose for rotation");
         return false;
    }

    double x, y, currentYaw;
    getPose(poseOdom, x, y, currentYaw);
    double requestedYaw = currentYaw + yaw;
    normalizeAngle(requestedYaw);

    return rotateAbs(requestedYaw);
}

// Rotate to specified orientation (in radians)

bool MoveBasic::rotateAbs(double requestedYaw)
{
    bool done = false;
    ros::Rate r(50);

    while (!done && ros::ok()) {
        ros::spinOnce();
        r.sleep();

        double x, y, currentYaw;
        tf2::Transform poseOdom;
        if (!getTransform("base_link", "odom", poseOdom)) {
             ROS_WARN("Cannot determine robot pose for rotation");
             return false;
        }
        getPose(poseOdom, x, y, currentYaw);

        double angleRemaining = requestedYaw - currentYaw;
        normalizeAngle(angleRemaining);

        double speed = std::max(minAngularVelocity,
            std::min(maxAngularVelocity,
              std::sqrt(2.0 * angularAcceleration * std::abs(angleRemaining))));

        double velocity = 0;

        if (angleRemaining < 0) {
            velocity = -speed;
        }
        else {
            velocity = speed;
        }

        if (actionServer->isNewGoalAvailable()) {
            ROS_INFO("Stopping rotation due to new goal");
            done = true;
            velocity = 0;
        }

        //ROS_INFO("%f %f %f", rad2deg(angleRemaining), angleRemaining, velocity);

        if (std::abs(angleRemaining) < angularTolerance) {
            velocity = 0;
            done = true;
            ROS_INFO("Done rotation, error %f radians %f degrees", angleRemaining, rad2deg(angleRemaining));
        }
        sendCmd(velocity, 0);
    }
    return done;
}

// Do linear part of goal

bool MoveBasic::handleLinear()
{
    bool done = false;

    tf2::Transform poseOdomInitial;
    if (!getTransform("base_link", "odom", poseOdomInitial)) {
         ROS_WARN("Cannot determine robot pose for linrar");
         return false;
    }

    tf2::Vector3 linear = goalOdom.getOrigin() - poseOdomInitial.getOrigin();
    double requestedDistance = linear.length();;
    ROS_INFO("Requested distance %f", requestedDistance);

    return moveLinear(requestedDistance);
}

// Move foreward specified distance

bool MoveBasic::moveLinear(double requestedDistance)
{
    bool done = false;
    ros::Rate r(50);

    tf2::Transform poseOdomInitial;
    if (!getTransform("base_link", "odom", poseOdomInitial)) {
         ROS_WARN("Cannot determine robot pose for linrar");
         return false;
    }

    while (!done && ros::ok()) {
        ros::spinOnce();
        r.sleep();

        tf2::Transform poseOdom;
        if (!getTransform("base_link", "odom", poseOdom)) {
             ROS_WARN("Cannot determine robot pose for linear");
             continue;
        }

        tf2::Vector3 travelled = poseOdomInitial.getOrigin() - poseOdom.getOrigin();
        double distTravelled = travelled.length();;

        double distRemaining = requestedDistance - distTravelled;

        double velocity = std::max(minLinearVelocity,
            std::min(maxLinearVelocity, std::min(
              std::sqrt(2.0 * linearAcceleration * std::abs(distTravelled)),
              std::sqrt(2.0 * linearAcceleration * std::abs(distRemaining)))));

        if (actionServer->isNewGoalAvailable()) {
            ROS_INFO("Stopping rotation due to new goal");
            done = true;
            velocity = 0;
        }

        //ROS_INFO("%f %f %f", distTravelled, distRemaining, velocity);

        if (distTravelled > requestedDistance - linearTolerance) {
            velocity = 0;
            done = true;
            ROS_INFO("Done linear, error %f meters", distRemaining);
        }
        sendCmd(0, velocity);
    }
    return done;
}

int main(int argc, char ** argv) {
    ros::init(argc, argv, "move_basic");
    MoveBasic mb_node;
    mb_node.run();

    return 0;
}
<commit_msg>Cleanup messages and comments<commit_after>/*
 * Copyright (c) 2017, Ubiquity Robotics
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. 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.
 *
 * 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.
 *
 * The views and conclusions contained in the software and documentation are
 * those of the authors and should not be interpreted as representing official
 * policies, either expressed or implied, of the FreeBSD Project.
 *
 */

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Path.h>

#include <actionlib/server/simple_action_server.h>
#include <move_base_msgs/MoveBaseAction.h>

#include <list>
#include <string>

typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;

class MoveBasic {
  private:
    ros::Subscriber goalSub;

    ros::Publisher goalPub;
    ros::Publisher cmdPub;
    ros::Publisher pathPub;

    std::unique_ptr<MoveBaseActionServer> actionServer;

    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener listener;

    double maxAngularVelocity;
    double minAngularVelocity;
    double angularAcceleration;
    double angularTolerance;

    double minLinearVelocity;
    double maxLinearVelocity;
    double linearAcceleration;
    double linearTolerance;

    tf2::Transform goalOdom;

    void goalCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);

    void executeAction(const move_base_msgs::MoveBaseGoalConstPtr& goal);

    void sendCmd(double angular, double linear);

    bool getTransform(const std::string& from, const std::string& to,
                      tf2::Transform& tf);

    bool handleRotation();
    bool handleLinear();

  public:
    MoveBasic();

    void run();

    bool moveLinear(double requestedDistance);
    bool rotateAbs(double requestedYaw);
    bool rotateRel(double yaw);
};

// Radians to degrees

static double rad2deg(double rad)
{
    return rad * 180.0 / M_PI;
}

// Adjust angle to be between -PI and PI

static void normalizeAngle(double& angle)
{
    if (angle < -M_PI) {
         angle += 2 * M_PI;
    }
    if (angle > M_PI) {
        angle -= 2 * M_PI;
    }
}

// retreive the 3 DOF we are interested in from a Transform

static void getPose(const tf2::Transform& tf, double& x, double& y, double& yaw)
{
    tf2::Vector3 trans = tf.getOrigin();
    x = trans.x();
    y = trans.y();

    double roll, pitch;
    tf.getBasis().getRPY(roll, pitch, yaw);
}

// Constructor

MoveBasic::MoveBasic(): tfBuffer(ros::Duration(30.0)),
                        listener(tfBuffer)
{
    ros::NodeHandle nh("~");

    nh.param<double>("min_angular_velocity", minAngularVelocity, 0.1);
    nh.param<double>("max_angular_velocity", maxAngularVelocity, 1.0);
    nh.param<double>("angular_acceleration", angularAcceleration, 0.3);
    nh.param<double>("angular_tolerance", angularTolerance, 0.01);

    nh.param<double>("min_linear_velocity", minLinearVelocity, 0.1);
    nh.param<double>("max_linear_velocity", maxLinearVelocity, 0.5);
    nh.param<double>("linear_acceleration", linearAcceleration, 0.5);
    nh.param<double>("linear_tolerance", linearTolerance, 0.01);

    cmdPub = ros::Publisher(nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1));

    pathPub = ros::Publisher(nh.advertise<nav_msgs::Path>("/plan", 1));

    goalSub = nh.subscribe("/move_base_simple/goal", 1,
                            &MoveBasic::goalCallback, this);

    ros::NodeHandle actionNh("");
    actionServer.reset(new MoveBaseActionServer(actionNh,
        "move_base", boost::bind(&MoveBasic::executeAction, this, _1), false));

    actionServer->start();
    goalPub = actionNh.advertise<move_base_msgs::MoveBaseActionGoal>(
      "/move_base/goal", 1);

    ROS_INFO("Move Basic ready");
}

// Lookup the specified transform, returns true on success

bool MoveBasic::getTransform(const std::string& from, const std::string& to,
                             tf2::Transform& tf)
{
    try {
        geometry_msgs::TransformStamped tfs =
            tfBuffer.lookupTransform(to, from, ros::Time(0));
        tf2::fromMsg(tfs.transform, tf);
        return true;
    }
    catch (tf2::TransformException &ex) {
         ROS_WARN("%s", ex.what());
         return false;
    }
}

// Called when a simple goal message is received

void MoveBasic::goalCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    ROS_INFO("Received simple goal");
    // send the goal to the action server
    move_base_msgs::MoveBaseActionGoal actionGoal;
    actionGoal.header.stamp = ros::Time::now();
    actionGoal.goal.target_pose = *msg;

    goalPub.publish(actionGoal);
}

// Called when an action goal is received
void MoveBasic::executeAction(const move_base_msgs::MoveBaseGoalConstPtr& msg)
{
    tf2::Transform goal;
    tf2::fromMsg(msg->target_pose.pose, goal);
    std::string frameId = msg->target_pose.header.frame_id;
    // Needed for RobotCommander
    if (frameId[0] == '/')
        frameId = frameId.substr(1);

    double x, y, yaw;
    getPose(goal, x, y, yaw);
    ROS_INFO("Received goal %f %f %f", x, y, rad2deg(yaw));

    tf2::Transform tfMapOdom;
    if (!getTransform(frameId, "odom", tfMapOdom)) {
        actionServer->setAborted(move_base_msgs::MoveBaseResult(),
                                 "Cannot determine robot pose");
        return;
    }
    goalOdom = tfMapOdom * goal;

    getPose(goalOdom, x, y, yaw);
    ROS_INFO("Goal in odom  %f %f %f", x, y, rad2deg(yaw));

    nav_msgs::Path path;
    geometry_msgs::PoseStamped p0, p1;
    path.header.frame_id = "odom";
    p0.pose.position.x = x;
    p0.pose.position.y = y;
    path.poses.push_back(p0);

    tf2::Transform poseOdom;
    if (!getTransform("base_link", "odom", poseOdom)) {
         actionServer->setAborted(move_base_msgs::MoveBaseResult(),
                                 "Cannot determine robot pose");
         return;
    }
    getPose(poseOdom, x, y, yaw);
    p1.pose.position.x = x;
    p1.pose.position.y = y;
    path.poses.push_back(p1);

    pathPub.publish(path);

    if (!handleRotation()) {
        actionServer->setAborted(move_base_msgs::MoveBaseResult(),
                                 "Rotation failed");
        return;
    }
    if (!handleLinear()) {
        actionServer->setAborted(move_base_msgs::MoveBaseResult(),
                                 "Linear movement failed");
        return;
    }
    getPose(goalOdom, x, y, yaw);
    rotateAbs(yaw);

    actionServer->setSucceeded();
}

// Send a motion command

void MoveBasic::sendCmd(double angular, double linear)
{
   geometry_msgs::Twist msg;
   msg.angular.z = angular;
   msg.linear.x = linear;

   cmdPub.publish(msg);
}

// Main loop

void MoveBasic::run()
{
    ros::Rate r(20);
    while (ros::ok()) {
        ros::spinOnce();
        r.sleep();
    }
}

// Do angular part of goal

bool MoveBasic::handleRotation()
{
    tf2::Transform poseOdom;
    if (!getTransform("base_link", "odom", poseOdom)) {
         ROS_WARN("Cannot determine robot pose for rotation");
         return false;
    }

    tf2::Vector3 linear = goalOdom.getOrigin() - poseOdom.getOrigin();
    // don't do initial rotation if there is no translation
    if (linear.length() == 0) {
        return true;
    }
    double requestedYaw = atan2(linear.y(), linear.x());

    if (requestedYaw == 0) {
        return true;
    }
    return rotateAbs(requestedYaw);
}

// Rotate relative to current orientation

bool MoveBasic::rotateRel(double yaw)
{
    tf2::Transform poseOdom;
    if (!getTransform("base_link", "odom", poseOdom)) {
         ROS_WARN("Cannot determine robot pose for rotation");
         return false;
    }

    double x, y, currentYaw;
    getPose(poseOdom, x, y, currentYaw);
    double requestedYaw = currentYaw + yaw;
    normalizeAngle(requestedYaw);

    return rotateAbs(requestedYaw);
}

// Rotate to specified orientation (in radians)

bool MoveBasic::rotateAbs(double requestedYaw)
{
    bool done = false;
    ros::Rate r(50);

    while (!done && ros::ok()) {
        ros::spinOnce();
        r.sleep();

        double x, y, currentYaw;
        tf2::Transform poseOdom;
        if (!getTransform("base_link", "odom", poseOdom)) {
             ROS_WARN("Cannot determine robot pose for rotation");
             return false;
        }
        getPose(poseOdom, x, y, currentYaw);

        double angleRemaining = requestedYaw - currentYaw;
        normalizeAngle(angleRemaining);

        double speed = std::max(minAngularVelocity,
            std::min(maxAngularVelocity,
              std::sqrt(2.0 * angularAcceleration * std::abs(angleRemaining))));

        double velocity = 0;

        if (angleRemaining < 0) {
            velocity = -speed;
        }
        else {
            velocity = speed;
        }

        if (actionServer->isNewGoalAvailable()) {
            ROS_INFO("Stopping rotation due to new goal");
            done = true;
            velocity = 0;
        }

        //ROS_INFO("%f %f %f", rad2deg(angleRemaining), angleRemaining, velocity);

        if (std::abs(angleRemaining) < angularTolerance) {
            velocity = 0;
            done = true;
            ROS_INFO("Done rotation, error %f degrees", rad2deg(angleRemaining));
        }
        sendCmd(velocity, 0);
    }
    return done;
}

// Do linear part of goal

bool MoveBasic::handleLinear()
{
    bool done = false;

    tf2::Transform poseOdomInitial;
    if (!getTransform("base_link", "odom", poseOdomInitial)) {
         ROS_WARN("Cannot determine robot pose for linear");
         return false;
    }

    tf2::Vector3 linear = goalOdom.getOrigin() - poseOdomInitial.getOrigin();
    double requestedDistance = linear.length();;
    ROS_INFO("Requested distance %f", requestedDistance);

    return moveLinear(requestedDistance);
}

// Move forward specified distance

bool MoveBasic::moveLinear(double requestedDistance)
{
    bool done = false;
    ros::Rate r(50);

    tf2::Transform poseOdomInitial;
    if (!getTransform("base_link", "odom", poseOdomInitial)) {
         ROS_WARN("Cannot determine robot pose for linear");
         return false;
    }

    while (!done && ros::ok()) {
        ros::spinOnce();
        r.sleep();

        tf2::Transform poseOdom;
        if (!getTransform("base_link", "odom", poseOdom)) {
             ROS_WARN("Cannot determine robot pose for linear");
             continue;
        }

        tf2::Vector3 travelled = poseOdomInitial.getOrigin() - poseOdom.getOrigin();
        double distTravelled = travelled.length();;

        double distRemaining = requestedDistance - distTravelled;

        double velocity = std::max(minLinearVelocity,
            std::min(maxLinearVelocity, std::min(
              std::sqrt(2.0 * linearAcceleration * std::abs(distTravelled)),
              std::sqrt(2.0 * linearAcceleration * std::abs(distRemaining)))));

        if (actionServer->isNewGoalAvailable()) {
            ROS_INFO("Stopping rotation due to new goal");
            done = true;
            velocity = 0;
        }

        //ROS_INFO("%f %f %f", distTravelled, distRemaining, velocity);

        if (distTravelled > requestedDistance - linearTolerance) {
            velocity = 0;
            done = true;
            ROS_INFO("Done linear, error %f meters", distRemaining);
        }
        sendCmd(0, velocity);
    }
    return done;
}

int main(int argc, char ** argv) {
    ros::init(argc, argv, "move_basic");
    MoveBasic mb_node;
    mb_node.run();

    return 0;
}
<|endoftext|>


<commit_before>/**
 *
 */
#include <iostream>
#include <sstream>

#include <zorba/zorba.h>

using namespace zorba;

bool
datamanager_example_1(Zorba* aZorba, XmlDataManager* aDataManager)
{
  try {
    std::stringstream lInStream("<books><book>Book 1</book><book>Book 2</book></books>");

    aDataManager->loadDocument("books.xml", lInStream);

	  XQuery_t lQuery = aZorba->compileQuery("doc('books.xml')//book");

    std::cout << lQuery << std::endl;
  } catch (ZorbaException& e) {
    std::cerr << e << std::endl;
    return false;
  }

	return true;
}

bool
datamanager_example_2(Zorba* aZorba, XmlDataManager* aDataManager)
{
  try {
    Collection_t lCollection =
      aDataManager->createCollection("http://www.flworfound.org/collections/mybooks");

    std::cout << "URI "  << lCollection->getUri().getStringValue() << std::endl;

    for (int i = 0; i < 10; ++i) {
      std::stringstream lInStream;
      lInStream << "<book>Book " << i << "</book>";
      lCollection->addDocument(lInStream);
    }

	  XQuery_t lQuery = aZorba->compileQuery("for $i in fn:collection()[3] return $i//book");

    Item lDefaultCollection = aZorba->getItemFactory()
      ->createAnyURI("http://www.flworfound.org/collections/mybooks");

    // set the default collection for this query
    lQuery->getDynamicContext()->setDefaultCollection(lDefaultCollection);

    std::cout << lQuery << std::endl;
  } catch (ZorbaException& e) {
    std::cerr << e << std::endl;
    return false;
  }

	return true;
}

bool
datamanager_example_3(Zorba* aZorba, XmlDataManager* aDataManager)
{
  try {
    Collection_t lCollection =
      aDataManager->getCollection("http://www.flworfound.org/collections/mybooks");

    std::stringstream lInStream;
    lInStream << "<book>Book";
    lCollection->addDocument(lInStream);

  } catch (ZorbaException& e) {
    std::cerr << e << std::endl;
    return true;
  }

	return false;
}

int
datamanager(int argc, char* argv[])
{
  Zorba* lZorba = Zorba::getInstance();
  XmlDataManager* lDataManager = lZorba->getXmlDataManager();

  std::cout << "executing example 1" << std::endl;
	assert(datamanager_example_1(lZorba, lDataManager));
  std::cout << std::endl;

  std::cout << "executing example 2" << std::endl;
	assert(datamanager_example_2(lZorba, lDataManager));
  std::cout << std::endl;

  std::cout << "executing example 3" << std::endl;
	assert(datamanager_example_3(lZorba, lDataManager));
  std::cout << std::endl;
  return 0;
}
<commit_msg>check if error is thrown if a collection already exists<commit_after>/**
 *
 */
#include <iostream>
#include <sstream>

#include <zorba/zorba.h>

using namespace zorba;

bool
datamanager_example_1(Zorba* aZorba, XmlDataManager* aDataManager)
{
  try {
    std::stringstream lInStream("<books><book>Book 1</book><book>Book 2</book></books>");

    aDataManager->loadDocument("books.xml", lInStream);

	  XQuery_t lQuery = aZorba->compileQuery("doc('books.xml')//book");

    std::cout << lQuery << std::endl;
  } catch (ZorbaException& e) {
    std::cerr << e << std::endl;
    return false;
  }

	return true;
}

bool
datamanager_example_2(Zorba* aZorba, XmlDataManager* aDataManager)
{
  try {
    Collection_t lCollection =
      aDataManager->createCollection("http://www.flworfound.org/collections/mybooks");

    std::cout << "URI "  << lCollection->getUri().getStringValue() << std::endl;

    for (int i = 0; i < 10; ++i) {
      std::stringstream lInStream;
      lInStream << "<book>Book " << i << "</book>";
      lCollection->addDocument(lInStream);
    }

	  XQuery_t lQuery = aZorba->compileQuery("for $i in fn:collection()[3] return $i//book");

    Item lDefaultCollection = aZorba->getItemFactory()
      ->createAnyURI("http://www.flworfound.org/collections/mybooks");

    // set the default collection for this query
    lQuery->getDynamicContext()->setDefaultCollection(lDefaultCollection);

    std::cout << lQuery << std::endl;
  } catch (ZorbaException& e) {
    std::cerr << e << std::endl;
    return false;
  }

	return true;
}

bool
datamanager_example_3(Zorba* aZorba, XmlDataManager* aDataManager)
{
  try {
    Collection_t lCollection =
      aDataManager->getCollection("http://www.flworfound.org/collections/mybooks");

    std::stringstream lInStream;
    lInStream << "<book>Book";
    lCollection->addDocument(lInStream);

  } catch (ZorbaException& e) {
    std::cerr << e << std::endl;
    return true;
  }

	return false;
}

bool
datamanager_example_4(Zorba* aZorba, XmlDataManager* aDataManager)
{
  try {
    // error if the collection already exists
    Collection_t lCollection =
      aDataManager->createCollection("http://www.flworfound.org/collections/mybooks");

  } catch (ZorbaException& e) {
    std::cerr << e << std::endl;
    return true;
  }

	return false;
}

int
datamanager(int argc, char* argv[])
{
  Zorba* lZorba = Zorba::getInstance();
  XmlDataManager* lDataManager = lZorba->getXmlDataManager();

  std::cout << "executing example 1" << std::endl;
	assert(datamanager_example_1(lZorba, lDataManager));
  std::cout << std::endl;

  std::cout << "executing example 2" << std::endl;
	assert(datamanager_example_2(lZorba, lDataManager));
  std::cout << std::endl;

  std::cout << "executing example 3" << std::endl;
	assert(datamanager_example_3(lZorba, lDataManager));
  std::cout << std::endl;

  std::cout << "executing example 4" << std::endl;
	assert(datamanager_example_4(lZorba, lDataManager));
  std::cout << std::endl;
  return 0;
}
<|endoftext|>


<commit_before>/****************************************************************/
/*             DO NOT MODIFY OR REMOVE THIS HEADER              */
/*          FALCON - Fracturing And Liquid CONvection           */
/*                                                              */
/*       (c) pending 2012 Battelle Energy Alliance, LLC         */
/*                   ALL RIGHTS RESERVED                        */
/*                                                              */
/*          Prepared by Battelle Energy Alliance, LLC           */
/*            Under Contract No. DE-AC07-05ID14517              */
/*            With the U. S. Department of Energy               */
/*                                                              */
/*            See COPYRIGHT for full restrictions               */
/****************************************************************/

#include "Material.h"
#include "EnthalpyConvectionWater.h"

template<>
InputParameters validParams<EnthalpyConvectionWater>()
{
    InputParameters params = validParams<Kernel>();
    params.addRequiredParam<std::string>("enthalpy_water", "material property, enthalpy of water");         //(added by Kat)
    //params.addRequiredCoupledVar("enthalpy_water", "Use CoupledAuxwater enthalpy here");                  (removed by Kat)
    //params.addRequiredCoupledVar("denthalpy_waterdH_P", "Use CoupledAux dsteamenthalpydh_P here");        (removed by Kat)
    //params.addRequiredCoupledVar("denthalpy_waterdP_H"," use coupledAux");                                (removed by Kat)
    params.addCoupledVar("pressure", "Use CoupledVar pressure here");
    return params;
}

EnthalpyConvectionWater::EnthalpyConvectionWater(const std::string & name, InputParameters parameters)
:Kernel(name, parameters),
    _Dtau_waterDH(getMaterialProperty<Real>("Dtau_waterDH")),
    _Dtau_waterDP(getMaterialProperty<Real>("Dtau_waterDP")),
    _darcy_mass_flux_water(getMaterialProperty<RealGradient>("darcy_mass_flux_water")),
    _tau_water(getMaterialProperty<Real>("tau_water")),
    //_enthalpy_water(coupledValue("enthalpy_water")),              (removed by Kat)
    //_denthalpy_waterdH_P(coupledValue("denthalpy_waterdH_P")),    (removed by Kat)
    //_denthalpy_waterdP_H(coupledValue("denthalpy_waterdP_H")),    (removed by Kat)
_prop_name_enthalpy_water(getParam<std::string>("enthalpy_water")),                  //(added by Kat)
    _enthalpy_water(getMaterialProperty<Real>(_prop_name_enthalpy_water)),           //(added by Kat)
_prop_name_denthalpy_waterdH_P(getParam<std::string>("denthalpy_waterdH_P")),        //(added by Kat)
    _denthalpy_waterdH_P(getMaterialProperty<Real>(_prop_name_denthalpy_waterdH_P)), //(added by Kat)
_prop_name_denthalpy_waterdP_H(getParam<std::string>("denthalpy_waterdP_H")),        //(added by Kat)
    _denthalpy_waterdP_H(getMaterialProperty<Real>(_prop_name_denthalpy_waterdP_H)), //(added by Kat)
    _p_var(coupled("pressure")),
    _grad_p(coupledGradient("pressure"))
{}

Real EnthalpyConvectionWater::computeQpResidual()
{

   // return  _darcy_mass_flux_water[_qp]*_grad_enthalpy_water[_qp]*_test[_i][_qp];
  return  -_darcy_mass_flux_water[_qp]*_enthalpy_water[_qp]*_grad_test[_i][_qp];
}

Real EnthalpyConvectionWater::computeQpJacobian()
{

    //return _darcy_mass_flux_water[_qp]*_denthalpy_waterdH_P[_qp]*_grad_phi[_j][_qp]*_test[_i][_qp];
  /* return -_grad_test[_i][_qp]*
            (_darcy_mass_flux_water[_qp]*_denthalpy_waterdH_P[_qp]*_phi[_j][_qp]
             +_Ddarcy_mass_flux_waterDH[_qp]*_enthalpy_water[_qp]*_phi[_j][_qp]);
  */
  return _grad_test[_i][_qp]*(_Dtau_waterDH[_qp]*_phi[_j][_qp]*_grad_p[_qp]*_enthalpy_water[_qp]
                              +_tau_water[_qp]*_grad_p[_qp]*_denthalpy_waterdH_P[_qp]*_phi[_j][_qp]);

}

Real EnthalpyConvectionWater::computeQpOffDiagJacobian(unsigned int jvar)
{
  if(jvar==_p_var)
  {//return _grad_test[_i][_qp]*(_tau_water[_qp]*_grad_phi[_j][_qp]*_enthalpy_water[_qp]);
    return _grad_test[_i][_qp]*(_Dtau_waterDP[_qp]*_phi[_j][_qp]*_grad_p[_qp]*_enthalpy_water[_qp]
                                +_tau_water[_qp]*_grad_phi[_j][_qp]*_enthalpy_water[_qp]
                               +_tau_water[_qp]*_grad_p[_qp]*_denthalpy_waterdP_H[_qp]*_phi[_j][_qp]);

  }
  else
  {return 0.0;
  }

}
<commit_msg>changed water/steam EOS coupled values to material properties<commit_after>/****************************************************************/
/*             DO NOT MODIFY OR REMOVE THIS HEADER              */
/*          FALCON - Fracturing And Liquid CONvection           */
/*                                                              */
/*       (c) pending 2012 Battelle Energy Alliance, LLC         */
/*                   ALL RIGHTS RESERVED                        */
/*                                                              */
/*          Prepared by Battelle Energy Alliance, LLC           */
/*            Under Contract No. DE-AC07-05ID14517              */
/*            With the U. S. Department of Energy               */
/*                                                              */
/*            See COPYRIGHT for full restrictions               */
/****************************************************************/

#include "Material.h"
#include "EnthalpyConvectionWater.h"

template<>
InputParameters validParams<EnthalpyConvectionWater>()
{
    InputParameters params = validParams<Kernel>();
    params.addRequiredParam<std::string>("enthalpy_water", "material property, enthalpy of water");         //(added by Kat)
    params.addRequiredParam("enthalpy_water", "Use CoupledAuxwater enthalpy here");                  (removed by Kat)
    params.addRequiredParam("denthalpy_waterdH_P", "Use CoupledAux dsteamenthalpydh_P here");        (removed by Kat)
    params.addRequiredParam("denthalpy_waterdP_H"," use coupledAux");                                (removed by Kat)
    params.addCoupledVar("pressure", "Use CoupledVar pressure here");
    return params;
}

EnthalpyConvectionWater::EnthalpyConvectionWater(const std::string & name, InputParameters parameters)
:Kernel(name, parameters),
    _Dtau_waterDH(getMaterialProperty<Real>("Dtau_waterDH")),
    _Dtau_waterDP(getMaterialProperty<Real>("Dtau_waterDP")),
    _darcy_mass_flux_water(getMaterialProperty<RealGradient>("darcy_mass_flux_water")),
    _tau_water(getMaterialProperty<Real>("tau_water")),
    //_enthalpy_water(coupledValue("enthalpy_water")),              (removed by Kat)
    //_denthalpy_waterdH_P(coupledValue("denthalpy_waterdH_P")),    (removed by Kat)
    //_denthalpy_waterdP_H(coupledValue("denthalpy_waterdP_H")),    (removed by Kat)
_prop_name_enthalpy_water(getParam<std::string>("enthalpy_water")),                  //(added by Kat)
    _enthalpy_water(getMaterialProperty<Real>(_prop_name_enthalpy_water)),           //(added by Kat)
_prop_name_denthalpy_waterdH_P(getParam<std::string>("denthalpy_waterdH_P")),        //(added by Kat)
    _denthalpy_waterdH_P(getMaterialProperty<Real>(_prop_name_denthalpy_waterdH_P)), //(added by Kat)
_prop_name_denthalpy_waterdP_H(getParam<std::string>("denthalpy_waterdP_H")),        //(added by Kat)
    _denthalpy_waterdP_H(getMaterialProperty<Real>(_prop_name_denthalpy_waterdP_H)), //(added by Kat)
    _p_var(coupled("pressure")),
    _grad_p(coupledGradient("pressure"))
{}

Real EnthalpyConvectionWater::computeQpResidual()
{

   // return  _darcy_mass_flux_water[_qp]*_grad_enthalpy_water[_qp]*_test[_i][_qp];
  return  -_darcy_mass_flux_water[_qp]*_enthalpy_water[_qp]*_grad_test[_i][_qp];
}

Real EnthalpyConvectionWater::computeQpJacobian()
{

    //return _darcy_mass_flux_water[_qp]*_denthalpy_waterdH_P[_qp]*_grad_phi[_j][_qp]*_test[_i][_qp];
  /* return -_grad_test[_i][_qp]*
            (_darcy_mass_flux_water[_qp]*_denthalpy_waterdH_P[_qp]*_phi[_j][_qp]
             +_Ddarcy_mass_flux_waterDH[_qp]*_enthalpy_water[_qp]*_phi[_j][_qp]);
  */
  return _grad_test[_i][_qp]*(_Dtau_waterDH[_qp]*_phi[_j][_qp]*_grad_p[_qp]*_enthalpy_water[_qp]
                              +_tau_water[_qp]*_grad_p[_qp]*_denthalpy_waterdH_P[_qp]*_phi[_j][_qp]);

}

Real EnthalpyConvectionWater::computeQpOffDiagJacobian(unsigned int jvar)
{
  if(jvar==_p_var)
  {//return _grad_test[_i][_qp]*(_tau_water[_qp]*_grad_phi[_j][_qp]*_enthalpy_water[_qp]);
    return _grad_test[_i][_qp]*(_Dtau_waterDP[_qp]*_phi[_j][_qp]*_grad_p[_qp]*_enthalpy_water[_qp]
                                +_tau_water[_qp]*_grad_phi[_j][_qp]*_enthalpy_water[_qp]
                               +_tau_water[_qp]*_grad_p[_qp]*_denthalpy_waterdP_H[_qp]*_phi[_j][_qp]);

  }
  else
  {return 0.0;
  }

}
<|endoftext|>


<commit_before>/**
 ** \file object/code-class.hh
 ** \brief Definition of the URBI object code.
 */

#ifndef OBJECT_CODE_CLASS_HH
# define OBJECT_CODE_CLASS_HH

# include <libport/compiler.hh>

# include <ast/fwd.hh>
# include <object/executable.hh>
# include <object/fwd.hh>

namespace object
{
  class Code: public Executable
  {
  public:
    typedef ast::rConstRoutine ast_type;
    typedef std::vector<rrObject> captures_type;

    Code(ast_type a);
    Code(rCode model);
    ast_type ast_get() const;
    rObject call_get() const;
    const captures_type& captures_get() const;
    rObject self_get() const;
    virtual rObject operator() (runner::Runner& r, object::objects_type args);

    ast_type& ast_get();
    rObject& call_get();
    captures_type& captures_get();
    rObject& self_get();

    /// Urbi methods
    rObject apply(runner::Runner& r, rList args);
    static std::string as_string(runner::Runner& r, rObject what);
    std::string body_string();

    virtual std::ostream& special_slots_dump (std::ostream& o,
					      runner::Runner&) const;

  private:
    /// Body of the function
    ast_type ast_;
    /// Value of the captured variables
    captures_type captures_;
    /// Captured 'this' and 'call'. Only set for closures.
    rObject self_, call_;

  URBI_CXX_OBJECT(Code);
  };
}; // namespace object

#endif // !OBJECT_CODE_CLASS_HH
<commit_msg>Use the right include.<commit_after>/**
 ** \file object/code-class.hh
 ** \brief Definition of the URBI object code.
 */

#ifndef OBJECT_CODE_CLASS_HH
# define OBJECT_CODE_CLASS_HH

# include <libport/compiler.hh>

# include <ast/routine.hh>
# include <object/executable.hh>
# include <object/fwd.hh>

namespace object
{
  class Code: public Executable
  {
  public:
    typedef ast::rConstRoutine ast_type;
    typedef std::vector<rrObject> captures_type;

    Code(ast_type a);
    Code(rCode model);
    ast_type ast_get() const;
    rObject call_get() const;
    const captures_type& captures_get() const;
    rObject self_get() const;
    virtual rObject operator() (runner::Runner& r, object::objects_type args);

    ast_type& ast_get();
    rObject& call_get();
    captures_type& captures_get();
    rObject& self_get();

    /// Urbi methods
    rObject apply(runner::Runner& r, rList args);
    static std::string as_string(runner::Runner& r, rObject what);
    std::string body_string();

    virtual std::ostream& special_slots_dump (std::ostream& o,
					      runner::Runner&) const;

  private:
    /// Body of the function
    ast_type ast_;
    /// Value of the captured variables
    captures_type captures_;
    /// Captured 'this' and 'call'. Only set for closures.
    rObject self_, call_;

  URBI_CXX_OBJECT(Code);
  };
}; // namespace object

#endif // !OBJECT_CODE_CLASS_HH
<|endoftext|>


<commit_before>/*******************************************************************************
 *
 * MIT License
 *
 * Copyright (c) 2017 Advanced Micro Devices, Inc.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 *******************************************************************************/
#include <../driver/tensor_driver.hpp>
#include <miopen/rnn.hpp>
#include <miopen/env.hpp>
#include <miopen/util.hpp>
#include <miopen/float_equal.hpp>
#include <vector>
#include <numeric>

#include <miopengemm/gemm.hpp>

namespace miopen {

MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_CONV_DIRECT)

struct AutoEnableProfiling
{
    AutoEnableProfiling(Handle& x) : h(x)
    {
        prev_state = h.IsProfilingEnabled();
        h.EnableProfiling();
    }

    ~AutoEnableProfiling()
    {
        h.EnableProfiling(prev_state);
        h.ResetKernelTime();
    }

    private:
    Handle& h;
    bool prev_state;
};

void RNNDescriptor::RNNForwardTraining(Handle& handle,
	const int seqLen,
//	const TensorDescriptor& xDesc,
	ConstData_t x,
//	const TensorDescriptor& hxDesc,
	ConstData_t hx,
//	const TensorDescriptor& cxDesc,
	ConstData_t cx,
//	const TensorDescriptor& wDesc,
	ConstData_t w,
//	const TensorDescriptor& yDesc,
	Data_t y,
//	const TensorDescriptor& hyDesc,
	Data_t hy,
//	const TensorDescriptor& cyDesc,
	Data_t cy,
	Data_t workSpace,
	size_t workSpaceSize,
	Data_t reserveSpace,
	size_t reserveSpaceSize,
	const std::vector<int> &in_n,
	const int in_h,
	const int hy_d,
	const int hy_n,
	const int hy_h,
	const int out_h) const
{/*
	if (x == nullptr || w == nullptr || y == nullptr)
	{
		MIOPEN_THROW(miopenStatusBadParm);
	}
	if (xDesc.GetSize() != yDesc.GetSize() || xDesc.GetSize() != wDesc.GetSize())
	{
		MIOPEN_THROW(miopenStatusBadParm);
	}
	if (xDesc.GetType() != yDesc.GetType() || xDesc.GetType() != wDesc.GetType())
	{
		MIOPEN_THROW(miopenStatusBadParm);
	}
*/
//	int in_n, in_c, in_h, in_w;
	//		std::tie(in_n, in_c, in_h, in_w) = tie4(xDesc.GetLengths());

//	int out_h, out_w;
	//		std::tie(std::ignore, std::ignore, out_h, out_w) = tie4(yDesc.GetLengths());
/*
	if (workSpace == nullptr ||
		workSpaceSize < ForwardGetWorkSpaceSize(handle, wDesc, xDesc, yDesc) || reserveSpaceSize < ForwardGetReserveSpaceSize(handle, wDesc, xDesc, yDesc))
	{
		MIOPEN_THROW("Workspace is required");
	}
	*/

//	miopenRNNMode_t mode;
//	int seqLength;
//	int layer;
//	int bidir;
//	int bias;

	int batch_n = std::accumulate(in_n.begin(), in_n.end(), 0);

	bool bidirection = (bidir != 0);
	bool biased = (bias != 0);
	int numlayer = layer;
	int bacc, baccbi;
	int bi = bidirection ? 2 : 1;

	int wei_len = (bi * (in_h + hy_h + out_h) + (numlayer - 1) * bi * (bi + 1) * hy_h) * hy_h;
	if (biased)
	{
		wei_len += (bi * 2 + (numlayer - 1) * bi * (bi + 1)) * hy_h + bi * out_h;
	}

	int wei_shift_bias =
		((in_h + hy_h + out_h) * bi + (bi * hy_h + hy_h) * bi * (numlayer - 1)) * hy_h;
	int in_stride = in_h;
	int hy_stride = hy_h * bi;
	int h_stride = hy_h * bi;
	int out_stride = out_h;
	int wei_stride = hy_h * bi;

	if (mode == miopenRNNRELU || mode == miopenRNNTANH)
	{
#if MIOPEN_USE_MIOPENGEMM

		printf("rnn gpu \n");
		cl_command_queue Q = (cl_command_queue)handle.GetStream();

		for (int li = 0; li < numlayer; li++)
		{
			int hid_shift = li * batch_n * hy_h * bi;
			int hx_shift = li * bi * in_n[0] * hy_h;

			// from input
			if (li == 0)
			{
				MIOpenGEMM::gemm0<float>(false,
					false,
					false,
					batch_n,
					hy_h * bi,
					in_h,
					1,
					x,
					0,
					in_stride,
					w,
					0,
					wei_stride,
					1,
					reserveSpace,
					hid_shift,
					hy_stride,
					&Q,
					0,
					nullptr,
					nullptr);
			}
			else
			{
				int wei_shift = bi * (in_h + hy_h) * hy_h + (li - 1) * bi * (bi * hy_h + hy_h) * hy_h;
				int prelayer_shift = (li - 1) * batch_n * hy_h * bi;

				MIOpenGEMM::gemm0<float>(false,
					false,
					false,
					batch_n,
					hy_h * bi,
					hy_h * bi,
					1,
					workSpace,
					prelayer_shift,
					hy_stride,
					w,
					wei_shift,
					wei_stride,
					1,
					reserveSpace,
					hid_shift,
					hy_stride,
					&Q,
					0,
					nullptr,
					nullptr);
			}

			// from hidden state
			bacc = 0;
			baccbi = batch_n;
			for (int ti = 0; ti < seqLen; ti++)
			{
				baccbi -= in_n[seqLen - 1 - ti];

				int wei_shift =
					li == 0 ? (in_h * hy_h * bi)
					: (bi * (in_h + hy_h) * hy_h + (li - 1) * bi * (bi * hy_h + hy_h) * hy_h +
						bi * hy_h * hy_stride);

				if (ti == 0)
				{
					MIOpenGEMM::gemm0<float>(false,
						false,
						false,
						in_n[ti],
						hy_h,
						hy_h,
						1,
						hx,
						hx_shift,
						h_stride,
						w,
						wei_shift,
						wei_stride,
						1,
						reserveSpace,
						hid_shift + bacc * hy_stride,
						hy_stride,
						&Q,
						0,
						nullptr,
						nullptr);

					if (bidirection)
					{
						MIOpenGEMM::gemm0<float>(false,
							false,
							false,
							in_n[seqLen - 1 - ti],
							hy_h,
							hy_h,
							1,
							hx,
							hx_shift + hy_h,
							h_stride,
							w,
							wei_shift + hy_h,
							wei_stride,
							1,
							reserveSpace,
							hid_shift + baccbi * hy_stride + hy_h,
							hy_stride,
							&Q,
							0,
							nullptr,
							nullptr);
					}
				}
				else
				{
					MIOpenGEMM::gemm0<float>(false,
						false,
						false,
						in_n[ti],
						hy_h,
						hy_h,
						1,
						hy,
						hx_shift,
						h_stride,
						w,
						wei_shift,
						wei_stride,
						1,
						reserveSpace,
						hid_shift + bacc * hy_stride,
						hy_stride,
						&Q,
						0,
						nullptr,
						nullptr);

					if (bidirection)
					{
						MIOpenGEMM::gemm0<float>(false,
							false,
							false,
							in_n[seqLen - 1 - ti],
							hy_h,
							hy_h,
							1,
							hy,
							hx_shift + hy_h,
							h_stride,
							w,
							wei_shift + hy_h,
							wei_stride,
							1,
							reserveSpace,
							hid_shift + baccbi * hy_stride + hy_h,
							hy_stride,
							&Q,
							0,
							nullptr,
							nullptr);
					}
				}

				int rsv_sz = batch_n * hy_d * hy_h;
				std::vector<int> rsv_size(3, 1);
				rsv_size.push_back(rsv_sz);

				miopenTensorDescriptor_t rsvTensor;
				miopenCreateTensorDescriptor(&rsvTensor);
				SetTensor4d(rsvTensor, rsv_size);

				miopenActivationDescriptor_t activDesc;
				miopenCreateActivationDescriptor(&activDesc);

				miopenActivationMode_t amode;
				amode = (mode == miopenRNNRELU) ? miopenActivationRELU : miopenActivationLOGISTIC;
				miopenSetActivationDescriptor(activDesc, amode, 1, 0, 1);

				miopenActivationForward(handle,
					activDesc,
					1,
					rsvTensor,
					reserveSpace,
					0,
					rsvTensor,
					workSpace);

				bacc += in_n[ti];
			}

		}

		int prelayer_shift = (numlayer - 1) * batch_n * hy_h * bi;
		int wei_shift = bi * (in_h + hy_h) * hy_h + (numlayer - 1) * bi * (bi * hy_h + hy_h) * hy_h;

		MIOpenGEMM::gemm0<float>(false,
			false,
			true,
			batch_n,
			out_h,
			hy_h * bi,
			1,
			workSpace,
			prelayer_shift,
			hy_stride,
			w,
			wei_shift,
			wei_stride,
			1,
			y,
			0,
			out_stride,
			&Q,
			0,
			nullptr,
			nullptr);

		clFinish(Q);
#else
		MIOPEN_THROW("GEMM is not supported");
#endif
	}
	else if (mode == miopenLSTM)
	{
		printf("lstm gpu \n");
	}
	else if (mode == miopenGRU)
	{
		printf("gru gpu \n");
	}

};

void RNNDescriptor::RNNBackwardData(Handle& handle,
	const int seqLen,
	const TensorDescriptor& yDesc,
	ConstData_t y,
	const TensorDescriptor& dyDesc,
	ConstData_t dy,
	const TensorDescriptor& dhyDesc,
	ConstData_t dhy,
	const TensorDescriptor& dcyDesc,
	ConstData_t dcy,
	const TensorDescriptor& wDesc,
	ConstData_t w,
	const TensorDescriptor& hxDesc,
	ConstData_t hx,
	const TensorDescriptor& cxDesc,
	ConstData_t cx,
	const TensorDescriptor& dxDesc,
	Data_t dx,
	const TensorDescriptor& dhxDesc,
	Data_t dhx,
	const TensorDescriptor& dcxDesc,
	Data_t dcx,
	Data_t workSpace,
	size_t workSpaceSize,
	ConstData_t reserveSpace,
	size_t reserveSpaceSize) const
{
	/*
	if (dx == nullptr || w == nullptr || dy == nullptr)
	{
		MIOPEN_THROW(miopenStatusBadParm);
	}
	if (dyDesc.GetSize() != dxDesc.GetSize() || dyDesc.GetSize() != wDesc.GetSize())
	{
		MIOPEN_THROW(miopenStatusBadParm);
	}
	if (dyDesc.GetType() != dxDesc.GetType() || dyDesc.GetType() != wDesc.GetType())
	{
		MIOPEN_THROW(miopenStatusBadParm);
	}

	*/

        int in_n, in_c, in_h, in_w;
	//		std::tie(in_n, in_c, in_h, in_w) = tie4(xDesc.GetLengths());

	int wei_n, wei_h, wei_w;
	//		std::tie(wei_n, std::ignore, wei_h, wei_w) = tie4(wDesc.GetLengths());

	int out_h, out_w;
	//		std::tie(std::ignore, std::ignore, out_h, out_w) = tie4(yDesc.GetLengths());
};

void RNNDescriptor::RNNBackwardWeights(Handle& handle,
	const int seqLen,
	const TensorDescriptor& xDesc,
	ConstData_t x,
	const TensorDescriptor& hxDesc,
	ConstData_t hx,
	const TensorDescriptor& dyDesc,
	ConstData_t dy,
	ConstData_t workSpace,
	size_t workSpaceSize,
	const TensorDescriptor& dwDesc,
	Data_t dw,
	ConstData_t reserveSpace,
	size_t reserveSpaceSize) const
{

        int in_n, in_c, in_h, in_w;
	//		std::tie(in_n, in_c, in_h, in_w) = tie4(xDesc.GetLengths());

	int wei_n, wei_h, wei_w;
	//		std::tie(wei_n, std::ignore, wei_h, wei_w) = tie4(wDesc.GetLengths());

	int out_h, out_w;
	//		std::tie(std::ignore, std::ignore, out_h, out_w) = tie4(yDesc.GetLengths());
};

} // namespace miopen
<commit_msg>activ...<commit_after>/*******************************************************************************
 *
 * MIT License
 *
 * Copyright (c) 2017 Advanced Micro Devices, Inc.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 *******************************************************************************/
#include <../driver/tensor_driver.hpp>
#include <miopen/tensor.hpp>
#include <miopen/rnn.hpp>
#include <miopen/env.hpp>
#include <miopen/util.hpp>
#include <miopen/float_equal.hpp>
#include <vector>
#include <numeric>

#include <miopengemm/gemm.hpp>

namespace miopen {

MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_CONV_DIRECT)

struct AutoEnableProfiling
{
    AutoEnableProfiling(Handle& x) : h(x)
    {
        prev_state = h.IsProfilingEnabled();
        h.EnableProfiling();
    }

    ~AutoEnableProfiling()
    {
        h.EnableProfiling(prev_state);
        h.ResetKernelTime();
    }

    private:
    Handle& h;
    bool prev_state;
};

void RNNDescriptor::RNNForwardTraining(Handle& handle,
	const int seqLen,
//	const TensorDescriptor& xDesc,
	ConstData_t x,
//	const TensorDescriptor& hxDesc,
	ConstData_t hx,
//	const TensorDescriptor& cxDesc,
	ConstData_t cx,
//	const TensorDescriptor& wDesc,
	ConstData_t w,
//	const TensorDescriptor& yDesc,
	Data_t y,
//	const TensorDescriptor& hyDesc,
	Data_t hy,
//	const TensorDescriptor& cyDesc,
	Data_t cy,
	Data_t workSpace,
	size_t workSpaceSize,
	Data_t reserveSpace,
	size_t reserveSpaceSize,
	const std::vector<int> &in_n,
	const int in_h,
	const int hy_d,
	const int hy_n,
	const int hy_h,
	const int out_h) const
{/*
	if (x == nullptr || w == nullptr || y == nullptr)
	{
		MIOPEN_THROW(miopenStatusBadParm);
	}
	if (xDesc.GetSize() != yDesc.GetSize() || xDesc.GetSize() != wDesc.GetSize())
	{
		MIOPEN_THROW(miopenStatusBadParm);
	}
	if (xDesc.GetType() != yDesc.GetType() || xDesc.GetType() != wDesc.GetType())
	{
		MIOPEN_THROW(miopenStatusBadParm);
	}
*/
//	int in_n, in_c, in_h, in_w;
	//		std::tie(in_n, in_c, in_h, in_w) = tie4(xDesc.GetLengths());

//	int out_h, out_w;
	//		std::tie(std::ignore, std::ignore, out_h, out_w) = tie4(yDesc.GetLengths());
/*
	if (workSpace == nullptr ||
		workSpaceSize < ForwardGetWorkSpaceSize(handle, wDesc, xDesc, yDesc) || reserveSpaceSize < ForwardGetReserveSpaceSize(handle, wDesc, xDesc, yDesc))
	{
		MIOPEN_THROW("Workspace is required");
	}
	*/

//	miopenRNNMode_t mode;
//	int seqLength;
//	int layer;
//	int bidir;
//	int bias;

	int batch_n = std::accumulate(in_n.begin(), in_n.end(), 0);

	bool bidirection = (bidir != 0);
	bool biased = (bias != 0);
	int numlayer = layer;
	int bacc, baccbi;
	int bi = bidirection ? 2 : 1;

	int wei_len = (bi * (in_h + hy_h + out_h) + (numlayer - 1) * bi * (bi + 1) * hy_h) * hy_h;
	if (biased)
	{
		wei_len += (bi * 2 + (numlayer - 1) * bi * (bi + 1)) * hy_h + bi * out_h;
	}

	int wei_shift_bias =
		((in_h + hy_h + out_h) * bi + (bi * hy_h + hy_h) * bi * (numlayer - 1)) * hy_h;
	int in_stride = in_h;
	int hy_stride = hy_h * bi;
	int h_stride = hy_h * bi;
	int out_stride = out_h;
	int wei_stride = hy_h * bi;

	if (mode == miopenRNNRELU || mode == miopenRNNTANH)
	{
#if MIOPEN_USE_MIOPENGEMM

		printf("rnn gpu \n");
		cl_command_queue Q = (cl_command_queue)handle.GetStream();

		for (int li = 0; li < numlayer; li++)
		{
			int hid_shift = li * batch_n * hy_h * bi;
			int hx_shift = li * bi * in_n[0] * hy_h;

			// from input
			if (li == 0)
			{
				MIOpenGEMM::gemm0<float>(false,
					false,
					false,
					batch_n,
					hy_h * bi,
					in_h,
					1,
					x,
					0,
					in_stride,
					w,
					0,
					wei_stride,
					1,
					reserveSpace,
					hid_shift,
					hy_stride,
					&Q,
					0,
					nullptr,
					nullptr);
			}
			else
			{
				int wei_shift = bi * (in_h + hy_h) * hy_h + (li - 1) * bi * (bi * hy_h + hy_h) * hy_h;
				int prelayer_shift = (li - 1) * batch_n * hy_h * bi;

				MIOpenGEMM::gemm0<float>(false,
					false,
					false,
					batch_n,
					hy_h * bi,
					hy_h * bi,
					1,
					workSpace,
					prelayer_shift,
					hy_stride,
					w,
					wei_shift,
					wei_stride,
					1,
					reserveSpace,
					hid_shift,
					hy_stride,
					&Q,
					0,
					nullptr,
					nullptr);
			}

			// from hidden state
			bacc = 0;
			baccbi = batch_n;
			for (int ti = 0; ti < seqLen; ti++)
			{
				baccbi -= in_n[seqLen - 1 - ti];

				int wei_shift =
					li == 0 ? (in_h * hy_h * bi)
					: (bi * (in_h + hy_h) * hy_h + (li - 1) * bi * (bi * hy_h + hy_h) * hy_h +
						bi * hy_h * hy_stride);

				if (ti == 0)
				{
					MIOpenGEMM::gemm0<float>(false,
						false,
						false,
						in_n[ti],
						hy_h,
						hy_h,
						1,
						hx,
						hx_shift,
						h_stride,
						w,
						wei_shift,
						wei_stride,
						1,
						reserveSpace,
						hid_shift + bacc * hy_stride,
						hy_stride,
						&Q,
						0,
						nullptr,
						nullptr);

					if (bidirection)
					{
						MIOpenGEMM::gemm0<float>(false,
							false,
							false,
							in_n[seqLen - 1 - ti],
							hy_h,
							hy_h,
							1,
							hx,
							hx_shift + hy_h,
							h_stride,
							w,
							wei_shift + hy_h,
							wei_stride,
							1,
							reserveSpace,
							hid_shift + baccbi * hy_stride + hy_h,
							hy_stride,
							&Q,
							0,
							nullptr,
							nullptr);
					}
				}
				else
				{
					MIOpenGEMM::gemm0<float>(false,
						false,
						false,
						in_n[ti],
						hy_h,
						hy_h,
						1,
						hy,
						hx_shift,
						h_stride,
						w,
						wei_shift,
						wei_stride,
						1,
						reserveSpace,
						hid_shift + bacc * hy_stride,
						hy_stride,
						&Q,
						0,
						nullptr,
						nullptr);

					if (bidirection)
					{
						MIOpenGEMM::gemm0<float>(false,
							false,
							false,
							in_n[seqLen - 1 - ti],
							hy_h,
							hy_h,
							1,
							hy,
							hx_shift + hy_h,
							h_stride,
							w,
							wei_shift + hy_h,
							wei_stride,
							1,
							reserveSpace,
							hid_shift + baccbi * hy_stride + hy_h,
							hy_stride,
							&Q,
							0,
							nullptr,
							nullptr);
					}
				}

				int rsv_sz = batch_n * hy_d * hy_h;
				std::vector<int> rsv_size(3, 1);
				rsv_size.push_back(rsv_sz);

				miopenTensorDescriptor_t rsvTensor;
				miopenCreateTensorDescriptor(&rsvTensor);
				SetTensor4d(rsvTensor, rsv_size);

				miopenActivationDescriptor_t activDesc;
				miopenCreateActivationDescriptor(&activDesc);

				miopenActivationMode_t amode;
				amode = (mode == miopenRNNRELU) ? miopenActivationRELU : miopenActivationLOGISTIC;
				miopenSetActivationDescriptor(activDesc, amode, 1, 0, 1);

				miopenActivationForward(handle,
					activDesc,
					1,
					rsvTensor,
					reserveSpace,
					0,
					rsvTensor,
					workSpace);

				bacc += in_n[ti];
			}

		}

		int prelayer_shift = (numlayer - 1) * batch_n * hy_h * bi;
		int wei_shift = bi * (in_h + hy_h) * hy_h + (numlayer - 1) * bi * (bi * hy_h + hy_h) * hy_h;

		MIOpenGEMM::gemm0<float>(false,
			false,
			true,
			batch_n,
			out_h,
			hy_h * bi,
			1,
			workSpace,
			prelayer_shift,
			hy_stride,
			w,
			wei_shift,
			wei_stride,
			1,
			y,
			0,
			out_stride,
			&Q,
			0,
			nullptr,
			nullptr);

		clFinish(Q);
#else
		MIOPEN_THROW("GEMM is not supported");
#endif
	}
	else if (mode == miopenLSTM)
	{
		printf("lstm gpu \n");
	}
	else if (mode == miopenGRU)
	{
		printf("gru gpu \n");
	}

};

void RNNDescriptor::RNNBackwardData(Handle& handle,
	const int seqLen,
	const TensorDescriptor& yDesc,
	ConstData_t y,
	const TensorDescriptor& dyDesc,
	ConstData_t dy,
	const TensorDescriptor& dhyDesc,
	ConstData_t dhy,
	const TensorDescriptor& dcyDesc,
	ConstData_t dcy,
	const TensorDescriptor& wDesc,
	ConstData_t w,
	const TensorDescriptor& hxDesc,
	ConstData_t hx,
	const TensorDescriptor& cxDesc,
	ConstData_t cx,
	const TensorDescriptor& dxDesc,
	Data_t dx,
	const TensorDescriptor& dhxDesc,
	Data_t dhx,
	const TensorDescriptor& dcxDesc,
	Data_t dcx,
	Data_t workSpace,
	size_t workSpaceSize,
	ConstData_t reserveSpace,
	size_t reserveSpaceSize) const
{
	/*
	if (dx == nullptr || w == nullptr || dy == nullptr)
	{
		MIOPEN_THROW(miopenStatusBadParm);
	}
	if (dyDesc.GetSize() != dxDesc.GetSize() || dyDesc.GetSize() != wDesc.GetSize())
	{
		MIOPEN_THROW(miopenStatusBadParm);
	}
	if (dyDesc.GetType() != dxDesc.GetType() || dyDesc.GetType() != wDesc.GetType())
	{
		MIOPEN_THROW(miopenStatusBadParm);
	}

	*/

        int in_n, in_c, in_h, in_w;
	//		std::tie(in_n, in_c, in_h, in_w) = tie4(xDesc.GetLengths());

	int wei_n, wei_h, wei_w;
	//		std::tie(wei_n, std::ignore, wei_h, wei_w) = tie4(wDesc.GetLengths());

	int out_h, out_w;
	//		std::tie(std::ignore, std::ignore, out_h, out_w) = tie4(yDesc.GetLengths());
};

void RNNDescriptor::RNNBackwardWeights(Handle& handle,
	const int seqLen,
	const TensorDescriptor& xDesc,
	ConstData_t x,
	const TensorDescriptor& hxDesc,
	ConstData_t hx,
	const TensorDescriptor& dyDesc,
	ConstData_t dy,
	ConstData_t workSpace,
	size_t workSpaceSize,
	const TensorDescriptor& dwDesc,
	Data_t dw,
	ConstData_t reserveSpace,
	size_t reserveSpaceSize) const
{

        int in_n, in_c, in_h, in_w;
	//		std::tie(in_n, in_c, in_h, in_w) = tie4(xDesc.GetLengths());

	int wei_n, wei_h, wei_w;
	//		std::tie(wei_n, std::ignore, wei_h, wei_w) = tie4(wDesc.GetLengths());

	int out_h, out_w;
	//		std::tie(std::ignore, std::ignore, out_h, out_w) = tie4(yDesc.GetLengths());
};

} // namespace miopen
<|endoftext|>


<commit_before>/***************************Copyright-DO-NOT-REMOVE-THIS-LINE**
 * CONDOR Copyright Notice
 *
 * See LICENSE.TXT for additional notices and disclaimers.
 *
 * Copyright (c)1990-1998 CONDOR Team, Computer Sciences Department,
 * University of Wisconsin-Madison, Madison, WI.  All Rights Reserved.
 * No use of the CONDOR Software Program Source Code is authorized
 * without the express consent of the CONDOR Team.  For more information
 * contact: CONDOR Team, Attention: Professor Miron Livny,
 * 7367 Computer Sciences, 1210 W. Dayton St., Madison, WI 53706-1685,
 * (608) 262-0856 or miron@cs.wisc.edu.
 *
 * U.S. Government Rights Restrictions: Use, duplication, or disclosure
 * by the U.S. Government is subject to restrictions as set forth in
 * subparagraph (c)(1)(ii) of The Rights in Technical Data and Computer
 * Software clause at DFARS 252.227-7013 or subparagraphs (c)(1) and
 * (2) of Commercial Computer Software-Restricted Rights at 48 CFR
 * 52.227-19, as applicable, CONDOR Team, Attention: Professor Miron
 * Livny, 7367 Computer Sciences, 1210 W. Dayton St., Madison,
 * WI 53706-1685, (608) 262-0856 or miron@cs.wisc.edu.
****************************Copyright-DO-NOT-REMOVE-THIS-LINE**/

/* Here is the version string - update before a public release */
/* --- IMPORTANT!  THE FORMAT OF THE VERSION STRING IS VERY STRICT
   BECAUSE IT IS PARSED AT RUNTIME.  DO NOT ALTER THE FORMAT OR ENTER
   ANYTHING EXTRA BEFORE THE DATE.  IF YOU WISH TO ADD EXTRA INFORMATION,
   DO SO _AFTER_ THE DATE AND BEFORE THE TRAILING '$' CHARACTER.
   EXAMPLES:
       $CondorVersion: 6.1.11 " __DATE__ " WinNTPreview $    [OK]
	   $CondorVersion: 6.1.11 WinNTPreview " __DATE__ " $	 [WRONG!!!]
   Any questions?  See Todd or Derek.  Note: if you mess it up, DaemonCore
   will EXCEPT at startup time.
*/
static char* CondorVersionString = "$CondorVersion: 6.1.13 " __DATE__ " $";

/*
   This is some wisdom from Cygnus's web page.  If you just try to use
   the "stringify" operator on a preprocessor directive, you'd get
   "PLATFORM", not "Intel Linux" (or whatever the value of PLATFORM
   is).  That's because the stringify operator is a special case, and
   the preprocessor isn't allowed to expand things that are passed to
   it.  However, by defining two layers of macros, you get the right
   behavior, since the first pass converts:

   xstr(PLATFORM) -> str(Intel Linux)

   and the next pass gives:

   str(Intel Linux) -> "Intel Linux"

   This is exactly what we want, so we use it.  -Derek Wright and Jeff
   Ballard, 12/2/99

   Also, because the NT build system is totally different, we have to
   define the correct platform string right in here. :( -Derek 12/3/99
*/

#if defined(WIN32)
#define PLATFORM INTEL-WINNT40
#endif

#define xstr(s) str(s)
#define str(s) #s

/* Here is the platform string.  You don't need to edit this */
static char* CondorPlatformString = "$CondorPlatform: " xstr(PLATFORM) " $";

extern "C" {

char*
CondorVersion( void )
{
	return CondorVersionString;
}

char*
CondorPlatform( void )
{
	return CondorPlatformString;
}

} /* extern "C" */

<commit_msg>6.1.14 version string (on the trunk).<commit_after>/***************************Copyright-DO-NOT-REMOVE-THIS-LINE**
 * CONDOR Copyright Notice
 *
 * See LICENSE.TXT for additional notices and disclaimers.
 *
 * Copyright (c)1990-1998 CONDOR Team, Computer Sciences Department,
 * University of Wisconsin-Madison, Madison, WI.  All Rights Reserved.
 * No use of the CONDOR Software Program Source Code is authorized
 * without the express consent of the CONDOR Team.  For more information
 * contact: CONDOR Team, Attention: Professor Miron Livny,
 * 7367 Computer Sciences, 1210 W. Dayton St., Madison, WI 53706-1685,
 * (608) 262-0856 or miron@cs.wisc.edu.
 *
 * U.S. Government Rights Restrictions: Use, duplication, or disclosure
 * by the U.S. Government is subject to restrictions as set forth in
 * subparagraph (c)(1)(ii) of The Rights in Technical Data and Computer
 * Software clause at DFARS 252.227-7013 or subparagraphs (c)(1) and
 * (2) of Commercial Computer Software-Restricted Rights at 48 CFR
 * 52.227-19, as applicable, CONDOR Team, Attention: Professor Miron
 * Livny, 7367 Computer Sciences, 1210 W. Dayton St., Madison,
 * WI 53706-1685, (608) 262-0856 or miron@cs.wisc.edu.
****************************Copyright-DO-NOT-REMOVE-THIS-LINE**/

/* Here is the version string - update before a public release */
/* --- IMPORTANT!  THE FORMAT OF THE VERSION STRING IS VERY STRICT
   BECAUSE IT IS PARSED AT RUNTIME.  DO NOT ALTER THE FORMAT OR ENTER
   ANYTHING EXTRA BEFORE THE DATE.  IF YOU WISH TO ADD EXTRA INFORMATION,
   DO SO _AFTER_ THE DATE AND BEFORE THE TRAILING '$' CHARACTER.
   EXAMPLES:
       $CondorVersion: 6.1.11 " __DATE__ " WinNTPreview $    [OK]
	   $CondorVersion: 6.1.11 WinNTPreview " __DATE__ " $	 [WRONG!!!]
   Any questions?  See Todd or Derek.  Note: if you mess it up, DaemonCore
   will EXCEPT at startup time.
*/
static char* CondorVersionString = "$CondorVersion: 6.1.14 " __DATE__ " $";

/*
   This is some wisdom from Cygnus's web page.  If you just try to use
   the "stringify" operator on a preprocessor directive, you'd get
   "PLATFORM", not "Intel Linux" (or whatever the value of PLATFORM
   is).  That's because the stringify operator is a special case, and
   the preprocessor isn't allowed to expand things that are passed to
   it.  However, by defining two layers of macros, you get the right
   behavior, since the first pass converts:

   xstr(PLATFORM) -> str(Intel Linux)

   and the next pass gives:

   str(Intel Linux) -> "Intel Linux"

   This is exactly what we want, so we use it.  -Derek Wright and Jeff
   Ballard, 12/2/99

   Also, because the NT build system is totally different, we have to
   define the correct platform string right in here. :( -Derek 12/3/99
*/

#if defined(WIN32)
#define PLATFORM INTEL-WINNT40
#endif

#define xstr(s) str(s)
#define str(s) #s

/* Here is the platform string.  You don't need to edit this */
static char* CondorPlatformString = "$CondorPlatform: " xstr(PLATFORM) " $";

extern "C" {

char*
CondorVersion( void )
{
	return CondorVersionString;
}

char*
CondorPlatform( void )
{
	return CondorPlatformString;
}

} /* extern "C" */

<|endoftext|>


<commit_before>//
//  ofxTerrain.cpp
//  displacementMap
//
//  Created by Xavier Fischer on 11/02/2015.
//
//
#include "ofMain.h"
#include "ofxTerrain.h"
#include "ofApp.h"

void ofxTerrain::setup() {
	setup(PROJECTOR_RESOLUTION_X, PROJECTOR_RESOLUTION_Y, 5, 30);
}

//--------------------------------------------------------------
void ofxTerrain::setup(int width, int height, int resolution, float velocity){

	defaultColor = ofFloatColor(1,1,1,0);//ofFloatColor(0,0,1,0);

	planeWidth = width;
	planeHeight = height;
	planeResolution = resolution;
	planeVelocity = velocity;
	meshSize = ofVec2f(ceilf(planeWidth/planeResolution), ceilf(planeHeight/planeResolution));
	sumDeltaX = 0;
	//heightMap.reserve(width*height);

	// ary[i][j] is then rewritten as ary[i*sizeY+j]

	planeZscale = 50.;

	noiseScale = 0.06;
	noiseSeed = 0.1;
	noiseAmp = 1;

	noiseScale2 = 0.13;
	noiseSeed2 = 0.3;
	noiseAmp2 = 0.15;
	drawYLines = false;

    mode = 3;
    speedRate = 0;

	setupLineMesh(meshSize.x, meshSize.y, planeResolution);

}

//--------------------------------------------------------------
ofFloatColor ofxTerrain::getColor(float a){
	a = a*4-1;
	return ofFloatColor(defaultColor.r,defaultColor.g,defaultColor.b,a); //a);
}

//--------------------------------------------------------------
void ofxTerrain::setupLineMesh(int width, int height, int segmentLength){

	mesh.setMode(OF_PRIMITIVE_POINTS);
	//
	mesh.setMode(OF_PRIMITIVE_LINES);
	//mesh.setMode(OF_PRIMITIVE_TRIANGLE_FAN);
	mesh.clear();
	mesh.enableColors();
	mesh.enableIndices();
	mesh.disableNormals();

	// setup height map
	for(int x=0; x<width; x++) {
		for(int y=0; y<height; y++) {

			if (mode == 0 || mode == 2) {
				float noiseValue = genNoise(x, y);
				heightMap.push_back(noiseValue);
			}
			else
			{
				float noiseValue = 1-noiseAmp2+genNoise2(x, y); // background noise only
				heightMap.push_back(noiseValue);
			}
		}
	}

	// setup vertices
	for (int x = 0; x < width; x++) {
		for (int y = 0; y < height; y++) {
			float Y = y * segmentLength;
			float X = x * segmentLength;
			float Z = heightMap[indexFromXY(x, y, height)];
			mesh.addVertex(ofVec3f(X,Y,Z * planeZscale));
			mesh.addColor(getColor(Z));
		}
	}

	// setup indexes
	for (int x = 0; x < width; x++) {
		for (int y = 0; y < height; y++) {

			// -
			if (x + 1 != width){
				mesh.addIndex(indexFromXY(x, y, height));
				mesh.addIndex(indexFromXY(x+1, y, height));
			}

			// |
			if (drawYLines) {
				if (y + 1 != height){
					mesh.addIndex(indexFromXY(x, y, height));
					mesh.addIndex(indexFromXY(x, y+1, height));
				}
			}
		}
	}
}

//--------------------------------------------------------------
float ofxTerrain::genNoise(int x, int y){
	float noiseValue = noiseAmp * ofNoise(x * noiseScale, y * noiseScale, noiseSeed);
	noiseValue += genNoise2(x,y);
	return noiseValue;
}

//--------------------------------------------------------------
float ofxTerrain::genNoise2(int x, int y){
	float noiseValue = ofNoise(x * noiseScale2, y * noiseScale2, noiseSeed2);
	if (noiseValue>0.9) {
		noiseValue *= noiseAmp2;
	} else {
		noiseValue = 0;
	}
	return noiseValue;
}

//--------------------------------------------------------------
void ofxTerrain::meshAppendX(int segmentLength, int numSegments){

	int newMeshSizeX = meshSize.x + numSegments;

	// setup height map
	//noiseScale = ofMap(ofGetMouseX(), 0 , ofGetWidth(), 0, 0.5);
	for(int x=meshSize.x; x<newMeshSizeX; x++) {
		for(int y=0; y<meshSize.y; y++) {
			switch (mode) {
			case 0:
				{
					float noiseValue = genNoise(x, y);
					heightMap.push_back(noiseValue);
				}
				break;
			case 1:
			case 3:
				{
					float noiseValue = 1-noiseAmp2+genNoise2(x, y); // background noise only
					heightMap.push_back(noiseValue);
				}
				break;
			}
		}
	}

	// setup vertices
	for(int x=meshSize.x; x<newMeshSizeX; x++) {
		for(int y=0; y<meshSize.y; y++) {

			float Y = y * segmentLength;
			float X = x * segmentLength;
			float Z = heightMap[indexFromXY(x, y, meshSize.y)];
			mesh.addVertex(ofVec3f(X,Y,Z * planeZscale));
			mesh.addColor(getColor(Z));
		}
	}

	// setup indexes
	for(int x=meshSize.x-1; x<newMeshSizeX; x++) {
		for (int y = 0; y < meshSize.y; y++) {

			float Y = y * segmentLength;

			// -
			if (x + 1 != newMeshSizeX){
				mesh.addIndex(indexFromXY(x, y, meshSize.y));
				mesh.addIndex(indexFromXY(x+1, y, meshSize.y));
			}

			// |
			if (drawYLines) {
				if (y + 1 != meshSize.y){
					mesh.addIndex(indexFromXY(x, y, meshSize.y));
					mesh.addIndex(indexFromXY(x, y+1, meshSize.y));
				}
			}
		}
	}

	meshSize.x += numSegments;

}

//--------------------------------------------------------------
void ofxTerrain::addHole(int x, int y){

	float nx = ofMap(x, 0, 1280, meshSize.x - planeWidth/planeResolution, meshSize.x);
	holes.push_back(ofVec2f(nx,y/planeResolution));
	holesAmp.push_back(0);
}

//--------------------------------------------------------------
void ofxTerrain::updateHoles(){

	for(int i = 0; i< holes.size(); i++) {

		ofVec2f pos = holes[i];
		float amp = holesAmp[i];
		float ampTween = ofxTween::map(amp, 0, VASA_HOLE_MAX_AMP, 0, VASA_HOLE_MAX_AMP, false, tweenEasing, ofxTween::easeInOut);

		if (amp> VASA_HOLE_MAX_AMP) continue;

		//   (coef d'amplitude)   x  exp^(  (Distance x coef d'ouverture) ^2)
		int xFrom = pos.x;
		int yFrom = pos.y;
		float holeRadius = VASA_HOLE_RADIUS;
		float radius = 2* holeRadius*holeRadius;

		float xMin = ofClamp(xFrom-radius, 0, meshSize.x);
		float xMax = ofClamp(xFrom+radius,0, meshSize.x);
		float xCenter = ofMap(0.5, 0, 1, xMin, xMax);

		float yMin = ofClamp(yFrom-radius, 0, meshSize.y);
		float yMax = ofClamp(yFrom+radius,0, meshSize.y);
		float yCenter = ofMap(0.5, 0, 1, yMin, yMax);

		for (int x = xMin; x < xMax; x++) {
			for (int y = yMin; y < yMax; y++) {
				int idx = indexFromXY(x, y, meshSize.y);
				ofVec3f curVertex = mesh.getVertex(idx);

				float val = ampTween * exp(- ( pow((float)x-xCenter,2)/radius + pow((float)y-yCenter,2)/radius));
				curVertex.z -= val;

				mesh.setVertex(idx, curVertex);
				mesh.setColor(idx, getColor((curVertex.z/planeZscale)));
			}
		}

		holesAmp[i] += 0.05;

	}

}

//--------------------------------------------------------------
void ofxTerrain::addHill(int x, int y, float radius){

	float nx = ofMap(x, 0, 1280, meshSize.x - planeWidth/planeResolution, meshSize.x);
	hills.push_back(ofVec2f(nx,y/planeResolution));
	hillsAmp.push_back(0);
	hillsRadius.push_back(radius);
}

//--------------------------------------------------------------
void ofxTerrain::updateHills(){

	for(int i = 0; i< hills.size(); i++) {

		ofVec2f pos = hills[i];
		float amp = hillsAmp[i];
		float ampTween = ofxTween::map(amp, 0, VASA_HILL_MAX_AMP, 0, VASA_HILL_MAX_AMP, true, tweenEasing, ofxTween::easeInOut);

		if (amp> VASA_HILL_MAX_AMP) continue;

		//   (coef d'amplitude)   x  exp^(  (Distance x coef d'ouverture) ^2)
		int xFrom = pos.x;
		int yFrom = pos.y;
		float hillRadius = hillsRadius[i];
		float radius = hillRadius*hillRadius;

		float xMin = ofClamp(xFrom-hillRadius, 0, meshSize.x);
		float xMax = ofClamp(xFrom+hillRadius,0, meshSize.x);
		float xCenter = ofMap(0.5, 0, 1, xMin, xMax);

		float yMin = ofClamp(yFrom-hillRadius, 0, meshSize.y);
		float yMax = ofClamp(yFrom+hillRadius,0, meshSize.y);
		float yCenter = ofMap(0.5, 0, 1, yMin, yMax);

		for (int x = xMin; x < xMax; x++) {
			for (int y = yMin; y < yMax; y++) {
				int idx = indexFromXY(x, y, meshSize.y);
				ofVec3f curVertex = mesh.getVertex(idx);

				//float val = ampTween * exp(- ( pow((float)x-xCenter,2)/radius + pow((float)y-yCenter,2)/radius));
				float sqrVal = radius - pow((float)x-xCenter,2) - pow((float)y-yCenter,2);
				if (sqrVal>0) {

					curVertex.z += amp * sqrt(sqrVal)/4;
				}

				mesh.setVertex(idx, curVertex);
				mesh.setColor(idx, getColor((curVertex.z/planeZscale)));
			}
		}

		hillsAmp[i] += 0.01;

	}

}

//--------------------------------------------------------------
int ofxTerrain::indexFromXY(int x, int y, int totalHeight){
	return x * totalHeight + y;
}

//--------------------------------------------------------------
void ofxTerrain::update(){

	switch (mode) {
	case 0:
	case 1:
	case 3:
		speedRate+=1/60.;
		double curDelta = speedRate*(planeVelocity);

		if (sumDeltaX > planeResolution){
			meshAppendX(planeResolution,1);
			sumDeltaX = 0;
		}

		sumDeltaX += curDelta-deltaX;
		deltaX = curDelta;

		break;
	}

	updateHoles();
	updateHills();
}

//--------------------------------------------------------------
void ofxTerrain::draw(){

	ofEnableDepthTest();

	ofClear(ofColor::fromHex(0x00008C));

	ofPushMatrix();
	ofTranslate(-deltaX+2.5*speedRate, 0);
	ofSetLineWidth(4);
	mesh.draw();

	ofPopMatrix();

	ofSetColor(ofColor::white); // draw text

	if (bSmallCursor){
		ofSetColor(ofColor::red);
		ofSetLineWidth(2);
		int lw = 2;
		ofLine(mouseX-lw, mouseY,mouseX+lw, mouseY);
		ofLine(mouseX, mouseY-lw,mouseX, mouseY+lw);
	}

	ofDisableDepthTest();
}

//--------------------------------------------------------------
void ofxTerrain::keyPressed(int key) {

	switch(key)
	{
	case ' ': { mode = (mode +1)  % 4;
			  } break;

	case 'h': addHole(800,400); break;
	case 'H': addHill(800,400,15); break;
	case 'j': addHole(ofGetMouseX(),ofGetMouseY()); break;
	case 'J': addHill(ofGetMouseX(),ofGetMouseY(),15); break;
	case 'k': addHill(ofGetMouseX(),ofGetMouseY(),5); break;
	case 'C' : bSmallCursor = !bSmallCursor; break;
	case 'B' : {
		/*ofxPostProcessing postTerrain;
		postTerrain.init(ofGetWidth(), ofGetHeight());
		postTerrain.createPass<FxaaPass>();
		postTerrain.createPass<FxaaPass>();
		postTerrain.createPass<BloomPass>();
		postList[VASA_MODE_TERRAIN] = postTerrain;
		ofxPostProcessing postDalles;
		postDalles.init(ofGetWidth(), ofGetHeight());
		postDalles.createPass<FxaaPass>();
		postDalles.createPass<FxaaPass>();
		postDalles.createPass<BloomPass>();
		postList[VASA_MODE_DALLES] = postDalles;*/
			   } break;
	case 'b' : {
		//ofxPostProcessing postTerrain;
		//postTerrain.init(ofGetWidth(), ofGetHeight());
		//postTerrain.createPass<FxaaPass>();
		//postTerrain.createPass<FxaaPass>();
		////postTerrain.createPass<BloomPass>();
		//postList[VASA_MODE_TERRAIN] = postTerrain;
		//ofxPostProcessing postDalles;
		//postDalles.init(ofGetWidth(), ofGetHeight());
		//postDalles.createPass<FxaaPass>();
		//postDalles.createPass<FxaaPass>();
		////postDalles.createPass<BloomPass>();
		//postList[VASA_MODE_DALLES] = postDalles;
			   } break;

	case 'd': {
		//
		ofApp *app = (ofApp *)ofxGetAppPtr();
		app->cam.setOrientation(ofVec3f(43.1984,0,0));
		app->cam.setPosition(-200, -200.867, 208.106);

			  } break;

        case 'm' : bSmallCursor = true; break;
        case 'M' : bSmallCursor=false; break;

	}

}<commit_msg>reset terrain<commit_after>//
//  ofxTerrain.cpp
//  displacementMap
//
//  Created by Xavier Fischer on 11/02/2015.
//
//
#include "ofMain.h"
#include "ofxTerrain.h"
#include "ofApp.h"

void ofxTerrain::setup() {
	setup(PROJECTOR_RESOLUTION_X, PROJECTOR_RESOLUTION_Y, 5, 30);
}

//--------------------------------------------------------------
void ofxTerrain::setup(int width, int height, int resolution, float velocity){

	defaultColor = ofFloatColor(1,1,1,0);//ofFloatColor(0,0,1,0);

	planeWidth = width;
	planeHeight = height;
	planeResolution = resolution;
	planeVelocity = velocity;
	meshSize = ofVec2f(ceilf(planeWidth/planeResolution), ceilf(planeHeight/planeResolution));
	sumDeltaX = 0;
	//heightMap.reserve(width*height);

	// ary[i][j] is then rewritten as ary[i*sizeY+j]

	planeZscale = 50.;

	noiseScale = 0.06;
	noiseSeed = 0.1;
	noiseAmp = 1;

	noiseScale2 = 0.13;
	noiseSeed2 = 0.3;
	noiseAmp2 = 0.15;
	drawYLines = false;

    mode = 3;
    speedRate = 0;

	setupLineMesh(meshSize.x, meshSize.y, planeResolution);

}

//--------------------------------------------------------------
ofFloatColor ofxTerrain::getColor(float a){
	a = a*4-1;
	return ofFloatColor(defaultColor.r,defaultColor.g,defaultColor.b,a); //a);
}

//--------------------------------------------------------------
void ofxTerrain::setupLineMesh(int width, int height, int segmentLength){

	mesh.setMode(OF_PRIMITIVE_POINTS);
	//
	mesh.setMode(OF_PRIMITIVE_LINES);
	//mesh.setMode(OF_PRIMITIVE_TRIANGLE_FAN);
	mesh.clear();
	mesh.enableColors();
	mesh.enableIndices();
	mesh.disableNormals();

	// setup height map
	for(int x=0; x<width; x++) {
		for(int y=0; y<height; y++) {

			if (mode == 0 || mode == 2) {
				float noiseValue = genNoise(x, y);
				heightMap.push_back(noiseValue);
			}
			else
			{
				float noiseValue = 1-noiseAmp2+genNoise2(x, y); // background noise only
				heightMap.push_back(noiseValue);
			}
		}
	}

	// setup vertices
	for (int x = 0; x < width; x++) {
		for (int y = 0; y < height; y++) {
			float Y = y * segmentLength;
			float X = x * segmentLength;
			float Z = heightMap[indexFromXY(x, y, height)];
			mesh.addVertex(ofVec3f(X,Y,Z * planeZscale));
			mesh.addColor(getColor(Z));
		}
	}

	// setup indexes
	for (int x = 0; x < width; x++) {
		for (int y = 0; y < height; y++) {

			// -
			if (x + 1 != width){
				mesh.addIndex(indexFromXY(x, y, height));
				mesh.addIndex(indexFromXY(x+1, y, height));
			}

			// |
			if (drawYLines) {
				if (y + 1 != height){
					mesh.addIndex(indexFromXY(x, y, height));
					mesh.addIndex(indexFromXY(x, y+1, height));
				}
			}
		}
	}
}

//--------------------------------------------------------------
float ofxTerrain::genNoise(int x, int y){
	float noiseValue = noiseAmp * ofNoise(x * noiseScale, y * noiseScale, noiseSeed);
	noiseValue += genNoise2(x,y);
	return noiseValue;
}

//--------------------------------------------------------------
float ofxTerrain::genNoise2(int x, int y){
	float noiseValue = ofNoise(x * noiseScale2, y * noiseScale2, noiseSeed2);
	if (noiseValue>0.9) {
		noiseValue *= noiseAmp2;
	} else {
		noiseValue = 0;
	}
	return noiseValue;
}

//--------------------------------------------------------------
void ofxTerrain::meshAppendX(int segmentLength, int numSegments){

	int newMeshSizeX = meshSize.x + numSegments;

	// setup height map
	//noiseScale = ofMap(ofGetMouseX(), 0 , ofGetWidth(), 0, 0.5);
	for(int x=meshSize.x; x<newMeshSizeX; x++) {
		for(int y=0; y<meshSize.y; y++) {
			switch (mode) {
			case 0:
				{
					float noiseValue = genNoise(x, y);
					heightMap.push_back(noiseValue);
				}
				break;
			case 1:
			case 3:
				{
					float noiseValue = 1-noiseAmp2+genNoise2(x, y); // background noise only
					heightMap.push_back(noiseValue);
				}
				break;
			}
		}
	}

	// setup vertices
	for(int x=meshSize.x; x<newMeshSizeX; x++) {
		for(int y=0; y<meshSize.y; y++) {

			float Y = y * segmentLength;
			float X = x * segmentLength;
			float Z = heightMap[indexFromXY(x, y, meshSize.y)];
			mesh.addVertex(ofVec3f(X,Y,Z * planeZscale));
			mesh.addColor(getColor(Z));
		}
	}

	// setup indexes
	for(int x=meshSize.x-1; x<newMeshSizeX; x++) {
		for (int y = 0; y < meshSize.y; y++) {

			float Y = y * segmentLength;

			// -
			if (x + 1 != newMeshSizeX){
				mesh.addIndex(indexFromXY(x, y, meshSize.y));
				mesh.addIndex(indexFromXY(x+1, y, meshSize.y));
			}

			// |
			if (drawYLines) {
				if (y + 1 != meshSize.y){
					mesh.addIndex(indexFromXY(x, y, meshSize.y));
					mesh.addIndex(indexFromXY(x, y+1, meshSize.y));
				}
			}
		}
	}

	meshSize.x += numSegments;

}

//--------------------------------------------------------------
void ofxTerrain::addHole(int x, int y){

	float nx = ofMap(x, 0, 1280, meshSize.x - planeWidth/planeResolution, meshSize.x);
	holes.push_back(ofVec2f(nx,y/planeResolution));
	holesAmp.push_back(0);
}

//--------------------------------------------------------------
void ofxTerrain::updateHoles(){

	for(int i = 0; i< holes.size(); i++) {

		ofVec2f pos = holes[i];
		float amp = holesAmp[i];
		float ampTween = ofxTween::map(amp, 0, VASA_HOLE_MAX_AMP, 0, VASA_HOLE_MAX_AMP, false, tweenEasing, ofxTween::easeInOut);

		if (amp> VASA_HOLE_MAX_AMP) continue;

		//   (coef d'amplitude)   x  exp^(  (Distance x coef d'ouverture) ^2)
		int xFrom = pos.x;
		int yFrom = pos.y;
		float holeRadius = VASA_HOLE_RADIUS;
		float radius = 2* holeRadius*holeRadius;

		float xMin = ofClamp(xFrom-radius, 0, meshSize.x);
		float xMax = ofClamp(xFrom+radius,0, meshSize.x);
		float xCenter = ofMap(0.5, 0, 1, xMin, xMax);

		float yMin = ofClamp(yFrom-radius, 0, meshSize.y);
		float yMax = ofClamp(yFrom+radius,0, meshSize.y);
		float yCenter = ofMap(0.5, 0, 1, yMin, yMax);

		for (int x = xMin; x < xMax; x++) {
			for (int y = yMin; y < yMax; y++) {
				int idx = indexFromXY(x, y, meshSize.y);
				ofVec3f curVertex = mesh.getVertex(idx);

				float val = ampTween * exp(- ( pow((float)x-xCenter,2)/radius + pow((float)y-yCenter,2)/radius));
				curVertex.z -= val;

				mesh.setVertex(idx, curVertex);
				mesh.setColor(idx, getColor((curVertex.z/planeZscale)));
			}
		}

		holesAmp[i] += 0.05;

	}

}

//--------------------------------------------------------------
void ofxTerrain::addHill(int x, int y, float radius){

	float nx = ofMap(x, 0, 1280, meshSize.x - planeWidth/planeResolution, meshSize.x);
	hills.push_back(ofVec2f(nx,y/planeResolution));
	hillsAmp.push_back(0);
	hillsRadius.push_back(radius);
}

//--------------------------------------------------------------
void ofxTerrain::updateHills(){

	for(int i = 0; i< hills.size(); i++) {

		ofVec2f pos = hills[i];
		float amp = hillsAmp[i];
		float ampTween = ofxTween::map(amp, 0, VASA_HILL_MAX_AMP, 0, VASA_HILL_MAX_AMP, true, tweenEasing, ofxTween::easeInOut);

		if (amp> VASA_HILL_MAX_AMP) continue;

		//   (coef d'amplitude)   x  exp^(  (Distance x coef d'ouverture) ^2)
		int xFrom = pos.x;
		int yFrom = pos.y;
		float hillRadius = hillsRadius[i];
		float radius = hillRadius*hillRadius;

		float xMin = ofClamp(xFrom-hillRadius, 0, meshSize.x);
		float xMax = ofClamp(xFrom+hillRadius,0, meshSize.x);
		float xCenter = ofMap(0.5, 0, 1, xMin, xMax);

		float yMin = ofClamp(yFrom-hillRadius, 0, meshSize.y);
		float yMax = ofClamp(yFrom+hillRadius,0, meshSize.y);
		float yCenter = ofMap(0.5, 0, 1, yMin, yMax);

		for (int x = xMin; x < xMax; x++) {
			for (int y = yMin; y < yMax; y++) {
				int idx = indexFromXY(x, y, meshSize.y);
				ofVec3f curVertex = mesh.getVertex(idx);

				//float val = ampTween * exp(- ( pow((float)x-xCenter,2)/radius + pow((float)y-yCenter,2)/radius));
				float sqrVal = radius - pow((float)x-xCenter,2) - pow((float)y-yCenter,2);
				if (sqrVal>0) {

					curVertex.z += amp * sqrt(sqrVal)/4;
				}

				mesh.setVertex(idx, curVertex);
				mesh.setColor(idx, getColor((curVertex.z/planeZscale)));
			}
		}

		hillsAmp[i] += 0.01;

	}

}

//--------------------------------------------------------------
int ofxTerrain::indexFromXY(int x, int y, int totalHeight){
	return x * totalHeight + y;
}

//--------------------------------------------------------------
void ofxTerrain::update(){

	switch (mode) {
	case 0:
	case 1:
	case 3:
		speedRate+=1/60.;
		double curDelta = speedRate*(planeVelocity);

		if (sumDeltaX > planeResolution){
			meshAppendX(planeResolution,1);
			sumDeltaX = 0;
		}

		sumDeltaX += curDelta-deltaX;
		deltaX = curDelta;

		break;
	}

	updateHoles();
	updateHills();
}

//--------------------------------------------------------------
void ofxTerrain::draw(){

	ofEnableDepthTest();

	ofClear(ofColor::fromHex(0x00008C));

	ofPushMatrix();
	ofTranslate(-deltaX+2.5*speedRate, 0);
	ofSetLineWidth(4);
	mesh.draw();

	ofPopMatrix();

	ofSetColor(ofColor::white); // draw text

	if (bSmallCursor){
		ofSetColor(ofColor::red);
		ofSetLineWidth(2);
		int lw = 2;
		ofLine(mouseX-lw, mouseY,mouseX+lw, mouseY);
		ofLine(mouseX, mouseY-lw,mouseX, mouseY+lw);
	}

	ofDisableDepthTest();
}

//--------------------------------------------------------------
void ofxTerrain::keyPressed(int key) {

	switch(key)
	{
	case ' ': { mode = (mode +1)  % 4;
			  } break;

	case 'h': addHole(800,400); break;
	case 'H': addHill(800,400,15); break;
	case 'j': addHole(ofGetMouseX(),ofGetMouseY()); break;
	case 'J': addHill(ofGetMouseX(),ofGetMouseY(),15); break;
	case 'k': addHill(ofGetMouseX(),ofGetMouseY(),5); break;
	case 'C' : bSmallCursor = !bSmallCursor; break;
	case 'B' : {
		/*ofxPostProcessing postTerrain;
		postTerrain.init(ofGetWidth(), ofGetHeight());
		postTerrain.createPass<FxaaPass>();
		postTerrain.createPass<FxaaPass>();
		postTerrain.createPass<BloomPass>();
		postList[VASA_MODE_TERRAIN] = postTerrain;
		ofxPostProcessing postDalles;
		postDalles.init(ofGetWidth(), ofGetHeight());
		postDalles.createPass<FxaaPass>();
		postDalles.createPass<FxaaPass>();
		postDalles.createPass<BloomPass>();
		postList[VASA_MODE_DALLES] = postDalles;*/
			   } break;
	case 'b' : {
		//ofxPostProcessing postTerrain;
		//postTerrain.init(ofGetWidth(), ofGetHeight());
		//postTerrain.createPass<FxaaPass>();
		//postTerrain.createPass<FxaaPass>();
		////postTerrain.createPass<BloomPass>();
		//postList[VASA_MODE_TERRAIN] = postTerrain;
		//ofxPostProcessing postDalles;
		//postDalles.init(ofGetWidth(), ofGetHeight());
		//postDalles.createPass<FxaaPass>();
		//postDalles.createPass<FxaaPass>();
		////postDalles.createPass<BloomPass>();
		//postList[VASA_MODE_DALLES] = postDalles;
			   } break;

	case 'd': {
		//
		ofApp *app = (ofApp *)ofxGetAppPtr();
		app->cam.setOrientation(ofVec3f(43.1984,0,0));
		app->cam.setPosition(-200, -200.867, 208.106);

			  } break;

        case 'm' : bSmallCursor = true; break;
        case 'M' : bSmallCursor=false; break;

        case 'r': setup(planeWidth, planeHeight, planeResolution, planeVelocity); break;

	}

}<|endoftext|>