<commit_before>//*****************************************************************************
// Copyright 2017-2020 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//*****************************************************************************
#include <numeric>

#include "ngraph/builder/split.hpp"
#include "ngraph/op/constant.hpp"
#include "ngraph/op/fused/split.hpp"
#include "ngraph/validation_util.hpp"

using namespace std;
using namespace ngraph;

constexpr NodeTypeInfo op::v0::Split::type_info;

op::v0::Split::Split(const Output<Node>& data, const Output<Node>& axis, const size_t num_split)
    : FusedOp({data, axis})
    , m_split_evenly{true}
    , m_num_split{num_split}
{
    constructor_validate_and_infer_types();
}

op::v0::Split::Split(const Output<Node>& data,
                     const Output<Node>& axis,
                     const std::vector<size_t>& splits)
    : FusedOp({data, axis})
    , m_split_evenly{false}
    , m_num_split{0}
    , m_splits{splits}
{
    constructor_validate_and_infer_types();
}

void op::v0::Split::pre_validate_and_infer_types()
{
    const auto axis_shape = input(1).get_shape();
    NODE_VALIDATION_CHECK(this, is_scalar(axis_shape), "The 'axis' input node must be scalar");

    const auto axis_node = input_value(1).get_node_shared_ptr();
    NODE_VALIDATION_CHECK(this, axis_node->is_constant(), "The 'axis' input node must be constant");
    const auto axis_node_const = as_type_ptr<op::Constant>(axis_node);
    m_axis = axis_node_const->get_vector<int64_t>()[0];

    // Create dynamic-typed outputs. Actual shape/type will be computed during shape inference
    for (size_t i = 0; i < std::max(m_splits.size(), m_num_split); i++)
    {
        set_output_type(i, input(0).get_element_type(), PartialShape::dynamic());
    }

    if (is_dynamic())
    {
        return;
    }

    const auto shape = input(0).get_shape();

    m_axis = ngraph::normalize_axis(this, m_axis, shape.size());

    const auto dimension_at_axis = shape.at(m_axis);
    if (m_split_evenly)
    {
        NODE_VALIDATION_CHECK(this,
                              dimension_at_axis % m_num_split == 0,
                              "The input tensor's dimension pointed by the 'axis' parameter: ",
                              dimension_at_axis,
                              " has to be a multiple of the 'num_split' parameter value: ",
                              m_num_split);

        m_splits.assign(m_num_split, dimension_at_axis / m_num_split);
    }
    else
    {
        const auto sum_splits = accumulate(begin(m_splits), end(m_splits), 0UL);
        NODE_VALIDATION_CHECK(this,
                              sum_splits == dimension_at_axis,
                              "The input tensor's dimension pointed by the 'axis' parameter: ",
                              dimension_at_axis,
                              " has to be equal to the sum of splits passed to the op: ",
                              sum_splits);

        const bool all_splits_positive =
            all_of(begin(m_splits), end(m_splits), [](const size_t v) { return v > 0; });

        NODE_VALIDATION_CHECK(this,
                              all_splits_positive == true,
                              "All values of the 'splits' attribute must be greater than zero");
    }
    set_input_is_relevant_to_shape(0);
}

NodeVector op::v0::Split::decompose_op() const
{
    return builder::split(input_value(0), m_splits, m_axis);
}

shared_ptr<Node> op::v0::Split::copy_with_new_args(const NodeVector& new_args) const
{
    check_new_args_count(this, new_args);
    return make_shared<Split>(new_args.at(0), new_args.at(1), m_splits);
}

constexpr NodeTypeInfo op::v1::Split::type_info;

op::v1::Split::Split(const Output<Node>& data, const Output<Node>& axis, const size_t num_splits)
    : FusedOp({data, axis})
    , m_num_splits{num_splits}
{
    constructor_validate_and_infer_types();
}

void op::v1::Split::validate_and_infer_types()
{
    const auto data_ps = input_value(0).get_partial_shape();
    const auto axis_ps = input_value(1).get_partial_shape();
    const auto axis_et = input_value(1).get_element_type();

    NODE_VALIDATION_CHECK(this,
                          axis_ps.rank().is_static() && (size_t)axis_ps.rank() == 0,
                          "The 'axis' input is expected to be a scalar. Got: ",
                          axis_ps);

    NODE_VALIDATION_CHECK(
        this, axis_et.is_integral(), "The 'axis' input only accepts integral types");

    if (input_value(1).get_node_shared_ptr()->is_constant())
    {
        const auto axis_input = as_type_ptr<op::Constant>(input_value(1).get_node_shared_ptr());
        auto axis = axis_input->cast_vector<int64_t>()[0];

        if (data_ps.is_static())
        {
            const auto data_shape = data_ps.to_shape();
            axis = ngraph::normalize_axis(this, axis, data_shape.size());

            const auto dimension_at_axis = data_shape.at(axis);

            NODE_VALIDATION_CHECK(this,
                                  dimension_at_axis % m_num_splits == 0,
                                  "The input tensor's dimension pointed by the 'axis' parameter: ",
                                  dimension_at_axis,
                                  " has to be a multiple of the 'num_splits' attribute value: ",
                                  m_num_splits);

            Shape each_output_shape{data_shape};
            each_output_shape.at(axis) = dimension_at_axis / m_num_splits;

            for (size_t i = 0; i < m_num_splits; ++i)
            {
                set_output_type(i, input(0).get_element_type(), each_output_shape);
            }
        }
    }
    else
    {
        for (size_t i = 0; i < m_num_splits; ++i)
        {
            set_output_type(i, input(0).get_element_type(), PartialShape::dynamic());
        }

        set_input_is_relevant_to_shape(0);
    }
}

shared_ptr<Node> op::v1::Split::copy_with_new_args(const NodeVector& new_args) const
{
    check_new_args_count(this, new_args);
    return make_shared<v1::Split>(new_args.at(0), new_args.at(1), m_num_splits);
}
<commit_msg>Setting outputs for dynamic v1::Split (#4256)<commit_after>//*****************************************************************************
// Copyright 2017-2020 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//*****************************************************************************
#include <numeric>

#include "ngraph/builder/split.hpp"
#include "ngraph/op/constant.hpp"
#include "ngraph/op/fused/split.hpp"
#include "ngraph/validation_util.hpp"

using namespace std;
using namespace ngraph;

constexpr NodeTypeInfo op::v0::Split::type_info;

op::v0::Split::Split(const Output<Node>& data, const Output<Node>& axis, const size_t num_split)
    : FusedOp({data, axis})
    , m_split_evenly{true}
    , m_num_split{num_split}
{
    constructor_validate_and_infer_types();
}

op::v0::Split::Split(const Output<Node>& data,
                     const Output<Node>& axis,
                     const std::vector<size_t>& splits)
    : FusedOp({data, axis})
    , m_split_evenly{false}
    , m_num_split{0}
    , m_splits{splits}
{
    constructor_validate_and_infer_types();
}

void op::v0::Split::pre_validate_and_infer_types()
{
    const auto axis_shape = input(1).get_shape();
    NODE_VALIDATION_CHECK(this, is_scalar(axis_shape), "The 'axis' input node must be scalar");

    const auto axis_node = input_value(1).get_node_shared_ptr();
    NODE_VALIDATION_CHECK(this, axis_node->is_constant(), "The 'axis' input node must be constant");
    const auto axis_node_const = as_type_ptr<op::Constant>(axis_node);
    m_axis = axis_node_const->get_vector<int64_t>()[0];

    // Create dynamic-typed outputs. Actual shape/type will be computed during shape inference
    for (size_t i = 0; i < std::max(m_splits.size(), m_num_split); i++)
    {
        set_output_type(i, input(0).get_element_type(), PartialShape::dynamic());
    }

    if (is_dynamic())
    {
        return;
    }

    const auto shape = input(0).get_shape();

    m_axis = ngraph::normalize_axis(this, m_axis, shape.size());

    const auto dimension_at_axis = shape.at(m_axis);
    if (m_split_evenly)
    {
        NODE_VALIDATION_CHECK(this,
                              dimension_at_axis % m_num_split == 0,
                              "The input tensor's dimension pointed by the 'axis' parameter: ",
                              dimension_at_axis,
                              " has to be a multiple of the 'num_split' parameter value: ",
                              m_num_split);

        m_splits.assign(m_num_split, dimension_at_axis / m_num_split);
    }
    else
    {
        const auto sum_splits = accumulate(begin(m_splits), end(m_splits), 0UL);
        NODE_VALIDATION_CHECK(this,
                              sum_splits == dimension_at_axis,
                              "The input tensor's dimension pointed by the 'axis' parameter: ",
                              dimension_at_axis,
                              " has to be equal to the sum of splits passed to the op: ",
                              sum_splits);

        const bool all_splits_positive =
            all_of(begin(m_splits), end(m_splits), [](const size_t v) { return v > 0; });

        NODE_VALIDATION_CHECK(this,
                              all_splits_positive == true,
                              "All values of the 'splits' attribute must be greater than zero");
    }
    set_input_is_relevant_to_shape(0);
}

NodeVector op::v0::Split::decompose_op() const
{
    return builder::split(input_value(0), m_splits, m_axis);
}

shared_ptr<Node> op::v0::Split::copy_with_new_args(const NodeVector& new_args) const
{
    check_new_args_count(this, new_args);
    return make_shared<Split>(new_args.at(0), new_args.at(1), m_splits);
}

constexpr NodeTypeInfo op::v1::Split::type_info;

op::v1::Split::Split(const Output<Node>& data, const Output<Node>& axis, const size_t num_splits)
    : FusedOp({data, axis})
    , m_num_splits{num_splits}
{
    constructor_validate_and_infer_types();
}

void op::v1::Split::validate_and_infer_types()
{
    const auto data_ps = input_value(0).get_partial_shape();
    const auto axis_ps = input_value(1).get_partial_shape();
    const auto axis_et = input_value(1).get_element_type();

    NODE_VALIDATION_CHECK(this,
                          axis_ps.rank().is_static() && (size_t)axis_ps.rank() == 0,
                          "The 'axis' input is expected to be a scalar. Got: ",
                          axis_ps);

    NODE_VALIDATION_CHECK(
        this, axis_et.is_integral(), "The 'axis' input only accepts integral types");

    if (input_value(1).get_node_shared_ptr()->is_constant() && data_ps.is_static())
    {
        const auto axis_input = as_type_ptr<op::Constant>(input_value(1).get_node_shared_ptr());
        auto axis = axis_input->cast_vector<int64_t>()[0];

        const auto data_shape = data_ps.to_shape();
        axis = ngraph::normalize_axis(this, axis, data_shape.size());

        const auto dimension_at_axis = data_shape.at(axis);

        NODE_VALIDATION_CHECK(this,
                              dimension_at_axis % m_num_splits == 0,
                              "The input tensor's dimension pointed by the 'axis' parameter: ",
                              dimension_at_axis,
                              " has to be a multiple of the 'num_splits' attribute value: ",
                              m_num_splits);

        Shape each_output_shape{data_shape};
        each_output_shape.at(axis) = dimension_at_axis / m_num_splits;

        for (size_t i = 0; i < m_num_splits; ++i)
        {
            set_output_type(i, input(0).get_element_type(), each_output_shape);
        }
    }
    else
    {
        for (size_t i = 0; i < m_num_splits; ++i)
        {
            set_output_type(i, input(0).get_element_type(), PartialShape::dynamic());
        }

        set_input_is_relevant_to_shape(0);
    }
}

shared_ptr<Node> op::v1::Split::copy_with_new_args(const NodeVector& new_args) const
{
    check_new_args_count(this, new_args);
    return make_shared<v1::Split>(new_args.at(0), new_args.at(1), m_num_splits);
}
<|endoftext|>


<commit_before>/* -------------------------------------------------------------------------- *
 *                      OpenSim:  HuntCrossleyForce.cpp                       *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2012 Stanford University and the Authors                *
 * Author(s): Peter Eastman                                                   *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

#include "HuntCrossleyForce.h"
#include "ContactGeometry.h"
#include "ContactGeometrySet.h"
#include "Model.h"
#include <OpenSim/Simulation/Model/BodySet.h>

namespace OpenSim {

//==============================================================================
//                          HUNT CROSSLEY FORCE
//==============================================================================
// Uses default (compiler-generated) destructor, copy constructor, copy
// assignment operator.

// Default constructor.
HuntCrossleyForce::HuntCrossleyForce()
{
    constructProperties();
}

// Take over ownership of supplied object.
HuntCrossleyForce::HuntCrossleyForce(ContactParameters* params)
{
    constructProperties();
    addContactParameters(params);
}

void HuntCrossleyForce::extendAddToSystem(SimTK::MultibodySystem& system) const
{
    Super::extendAddToSystem(system);

    const ContactParametersSet& contactParametersSet =
        get_contact_parameters();
    const double& transitionVelocity = get_transition_velocity();

    SimTK::GeneralContactSubsystem& contacts = system.updContactSubsystem();
    SimTK::SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
    SimTK::ContactSetIndex set = contacts.createContactSet();
    SimTK::HuntCrossleyForce force(_model->updForceSubsystem(), contacts, set);
    force.setTransitionVelocity(transitionVelocity);
    for (int i = 0; i < contactParametersSet.getSize(); ++i)
    {
        ContactParameters& params = contactParametersSet.get(i);
        for (int j = 0; j < params.getGeometry().size(); ++j)
        {
            if (!_model->updContactGeometrySet().contains(params.getGeometry()[j]))
            {
                std::string errorMessage = "Invalid ContactGeometry (" + params.getGeometry()[j] + ") specified in HuntCrossleyForce" + getName();
                throw Exception(errorMessage);
            }
            ContactGeometry& geom = _model->updContactGeometrySet().get(params.getGeometry()[j]);
            contacts.addBody(set, geom.getBody().getMobilizedBody(), geom.createSimTKContactGeometry(), geom.getTransform());
            force.setBodyParameters(SimTK::ContactSurfaceIndex(contacts.getNumBodies(set)-1), params.getStiffness(), params.getDissipation(),
                params.getStaticFriction(), params.getDynamicFriction(), params.getViscousFriction());
        }
    }

    // Beyond the const Component get the index so we can access the SimTK::Force later
    HuntCrossleyForce* mutableThis = const_cast<HuntCrossleyForce *>(this);
    mutableThis->_index = force.getForceIndex();
}

void HuntCrossleyForce::constructProperties()
{
    constructProperty_contact_parameters(ContactParametersSet());
    constructProperty_transition_velocity(0.01);
}

HuntCrossleyForce::ContactParametersSet& HuntCrossleyForce::
updContactParametersSet()
{
    return upd_contact_parameters();
}

const HuntCrossleyForce::ContactParametersSet& HuntCrossleyForce::
getContactParametersSet()
{
    return get_contact_parameters();
}

void HuntCrossleyForce::
addContactParameters(HuntCrossleyForce::ContactParameters* params)
{
    updContactParametersSet().adoptAndAppend(params);
}

double HuntCrossleyForce::getTransitionVelocity() const
{
    return get_transition_velocity();
}

void HuntCrossleyForce::setTransitionVelocity(double velocity)
{
    set_transition_velocity(velocity);
}
/**
 * The following set of functions are introduced for convenience to get/set values in HuntCrossleyForce::ContactParameters
 * and for access in Matlab without exposing HuntCrossleyForce::ContactParameters. pending refactoring contact forces
 */
double HuntCrossleyForce::getStiffness()  {
    if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
    return get_contact_parameters().get(0).getStiffness();
};
void HuntCrossleyForce::setStiffness(double stiffness) {
        if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
        upd_contact_parameters()[0].setStiffness(stiffness);
};
double HuntCrossleyForce::getDissipation()   {
    if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
    return get_contact_parameters().get(0).getDissipation();
};
void HuntCrossleyForce::setDissipation(double dissipation) {
        if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
        upd_contact_parameters()[0].setDissipation(dissipation);
};
double HuntCrossleyForce::getStaticFriction()  {
    if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
    return get_contact_parameters().get(0).getStaticFriction();
};
void HuntCrossleyForce::setStaticFriction(double friction) {
        if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
        upd_contact_parameters()[0].setStaticFriction(friction);
};
double HuntCrossleyForce::getDynamicFriction()   {
    if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
    return get_contact_parameters().get(0).getDynamicFriction();
};
void HuntCrossleyForce::setDynamicFriction(double friction) {
        if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
        upd_contact_parameters()[0].setDynamicFriction(friction);
};
double HuntCrossleyForce::getViscousFriction()   {
    if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
    return get_contact_parameters().get(0).getViscousFriction();
};
void HuntCrossleyForce::setViscousFriction(double friction) {
        if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
        upd_contact_parameters()[0].setViscousFriction(friction);
};

void HuntCrossleyForce::addGeometry(const std::string& name)
{
    if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
    upd_contact_parameters()[0].addGeometry(name);
}
//==============================================================================
//                HUNT CROSSLEY FORCE :: CONTACT PARAMETERS
//==============================================================================

// Default constructor.
HuntCrossleyForce::ContactParameters::ContactParameters()
{
    constructProperties();
}

// Constructor specifying material properties.
HuntCrossleyForce::ContactParameters::ContactParameters
   (double stiffness, double dissipation, double staticFriction,
    double dynamicFriction, double viscousFriction)
{
    constructProperties();
    set_stiffness(stiffness);
    set_dissipation(dissipation);
    set_static_friction(staticFriction);
    set_dynamic_friction(dynamicFriction);
    set_viscous_friction(viscousFriction);
}

void HuntCrossleyForce::ContactParameters::constructProperties()
{
    constructProperty_geometry(); // a list of strings
    constructProperty_stiffness(0.0);
    constructProperty_dissipation(0.0);
    constructProperty_static_friction(0.0);
    constructProperty_dynamic_friction(0.0);
    constructProperty_viscous_friction(0.0);
}

const Property<std::string>& HuntCrossleyForce::ContactParameters::getGeometry() const
{
    return getProperty_geometry();
}

Property<std::string>& HuntCrossleyForce::ContactParameters::updGeometry()
{
    return updProperty_geometry();
}

void HuntCrossleyForce::ContactParameters::addGeometry(const std::string& name)
{
    updGeometry().appendValue(name);
}

double HuntCrossleyForce::ContactParameters::getStiffness() const
{
    return get_stiffness();
}

void HuntCrossleyForce::ContactParameters::setStiffness(double stiffness)
{
    set_stiffness(stiffness);
}

double HuntCrossleyForce::ContactParameters::getDissipation() const
{
    return get_dissipation();
}

void HuntCrossleyForce::ContactParameters::setDissipation(double dissipation)
{
    set_dissipation(dissipation);
}

double HuntCrossleyForce::ContactParameters::getStaticFriction() const
{
    return get_static_friction();
}

void HuntCrossleyForce::ContactParameters::setStaticFriction(double friction)
{
    set_static_friction(friction);
}

double HuntCrossleyForce::ContactParameters::getDynamicFriction() const
{
    return get_dynamic_friction();
}

void HuntCrossleyForce::ContactParameters::setDynamicFriction(double friction)
{
    set_dynamic_friction(friction);
}

double HuntCrossleyForce::ContactParameters::getViscousFriction() const
{
    return get_viscous_friction();
}

void HuntCrossleyForce::ContactParameters::setViscousFriction(double friction)
{
    set_viscous_friction(friction);
}

void HuntCrossleyForce::ContactParametersSet::setNull()
{
    setAuthors("Peter Eastman");
}

HuntCrossleyForce::ContactParametersSet::ContactParametersSet()
{
    setNull();
}

//=============================================================================
// Reporting
//=============================================================================
/**
 * Provide names of the quantities (column labels) of the force value(s) reported
 *
 */
OpenSim::Array<std::string> HuntCrossleyForce::getRecordLabels() const
{
    OpenSim::Array<std::string> labels("");

    const ContactParametersSet& contactParametersSet =
        get_contact_parameters();

    for (int i = 0; i < contactParametersSet.getSize(); ++i)
    {
        ContactParameters& params = contactParametersSet.get(i);
        for (int j = 0; j < params.getGeometry().size(); ++j)
        {
            ContactGeometry& geom = _model->updContactGeometrySet().get(params.getGeometry()[j]);
            std::string bodyName = geom.getBodyName();
            labels.append(getName()+"."+bodyName+".force.X");
            labels.append(getName()+"."+bodyName+".force.Y");
            labels.append(getName()+"."+bodyName+".force.Z");
            labels.append(getName()+"."+bodyName+".torque.X");
            labels.append(getName()+"."+bodyName+".torque.Y");
            labels.append(getName()+"."+bodyName+".torque.Z");
        }
    }

    return labels;
}
/**
 * Provide the value(s) to be reported that correspond to the labels
 */
OpenSim::Array<double> HuntCrossleyForce::
getRecordValues(const SimTK::State& state) const
{
    OpenSim::Array<double> values(1);

    const ContactParametersSet& contactParametersSet =
        get_contact_parameters();

    const SimTK::HuntCrossleyForce& simtkForce =
        (SimTK::HuntCrossleyForce &)(_model->getForceSubsystem().getForce(_index));

    SimTK::Vector_<SimTK::SpatialVec> bodyForces(0);
    SimTK::Vector_<SimTK::Vec3> particleForces(0);
    SimTK::Vector mobilityForces(0);

    //get the net force added to the system contributed by the Spring
    simtkForce.calcForceContribution(state, bodyForces, particleForces,
                                     mobilityForces);

    for (int i = 0; i < contactParametersSet.getSize(); ++i)
    {
        ContactParameters& params = contactParametersSet.get(i);
        for (int j = 0; j < params.getGeometry().size(); ++j)
        {
            ContactGeometry& geom =
                _model->updContactGeometrySet().get(params.getGeometry()[j]);
            std::string bodyName = geom.getBodyName();

            SimTK::Vec3 forces =
                bodyForces(geom.getBody().getMobilizedBodyIndex())[1];
            SimTK::Vec3 torques =
                bodyForces(geom.getBody().getMobilizedBodyIndex())[0];

            values.append(3, &forces[0]);
            values.append(3, &torques[0]);
        }
    }

    return values;
}

}// end of namespace OpenSim
<commit_msg>Unused variable.<commit_after>/* -------------------------------------------------------------------------- *
 *                      OpenSim:  HuntCrossleyForce.cpp                       *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2012 Stanford University and the Authors                *
 * Author(s): Peter Eastman                                                   *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

#include "HuntCrossleyForce.h"
#include "ContactGeometry.h"
#include "ContactGeometrySet.h"
#include "Model.h"
#include <OpenSim/Simulation/Model/BodySet.h>

namespace OpenSim {

//==============================================================================
//                          HUNT CROSSLEY FORCE
//==============================================================================
// Uses default (compiler-generated) destructor, copy constructor, copy
// assignment operator.

// Default constructor.
HuntCrossleyForce::HuntCrossleyForce()
{
    constructProperties();
}

// Take over ownership of supplied object.
HuntCrossleyForce::HuntCrossleyForce(ContactParameters* params)
{
    constructProperties();
    addContactParameters(params);
}

void HuntCrossleyForce::extendAddToSystem(SimTK::MultibodySystem& system) const
{
    Super::extendAddToSystem(system);

    const ContactParametersSet& contactParametersSet =
        get_contact_parameters();
    const double& transitionVelocity = get_transition_velocity();

    SimTK::GeneralContactSubsystem& contacts = system.updContactSubsystem();
    //SimTK::SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
    SimTK::ContactSetIndex set = contacts.createContactSet();
    SimTK::HuntCrossleyForce force(_model->updForceSubsystem(), contacts, set);
    force.setTransitionVelocity(transitionVelocity);
    for (int i = 0; i < contactParametersSet.getSize(); ++i)
    {
        ContactParameters& params = contactParametersSet.get(i);
        for (int j = 0; j < params.getGeometry().size(); ++j)
        {
            if (!_model->updContactGeometrySet().contains(params.getGeometry()[j]))
            {
                std::string errorMessage = "Invalid ContactGeometry (" + params.getGeometry()[j] + ") specified in HuntCrossleyForce" + getName();
                throw Exception(errorMessage);
            }
            ContactGeometry& geom = _model->updContactGeometrySet().get(params.getGeometry()[j]);
            contacts.addBody(set, geom.getBody().getMobilizedBody(), geom.createSimTKContactGeometry(), geom.getTransform());
            force.setBodyParameters(SimTK::ContactSurfaceIndex(contacts.getNumBodies(set)-1), params.getStiffness(), params.getDissipation(),
                params.getStaticFriction(), params.getDynamicFriction(), params.getViscousFriction());
        }
    }

    // Beyond the const Component get the index so we can access the SimTK::Force later
    HuntCrossleyForce* mutableThis = const_cast<HuntCrossleyForce *>(this);
    mutableThis->_index = force.getForceIndex();
}

void HuntCrossleyForce::constructProperties()
{
    constructProperty_contact_parameters(ContactParametersSet());
    constructProperty_transition_velocity(0.01);
}

HuntCrossleyForce::ContactParametersSet& HuntCrossleyForce::
updContactParametersSet()
{
    return upd_contact_parameters();
}

const HuntCrossleyForce::ContactParametersSet& HuntCrossleyForce::
getContactParametersSet()
{
    return get_contact_parameters();
}

void HuntCrossleyForce::
addContactParameters(HuntCrossleyForce::ContactParameters* params)
{
    updContactParametersSet().adoptAndAppend(params);
}

double HuntCrossleyForce::getTransitionVelocity() const
{
    return get_transition_velocity();
}

void HuntCrossleyForce::setTransitionVelocity(double velocity)
{
    set_transition_velocity(velocity);
}
/**
 * The following set of functions are introduced for convenience to get/set values in HuntCrossleyForce::ContactParameters
 * and for access in Matlab without exposing HuntCrossleyForce::ContactParameters. pending refactoring contact forces
 */
double HuntCrossleyForce::getStiffness()  {
    if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
    return get_contact_parameters().get(0).getStiffness();
};
void HuntCrossleyForce::setStiffness(double stiffness) {
        if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
        upd_contact_parameters()[0].setStiffness(stiffness);
};
double HuntCrossleyForce::getDissipation()   {
    if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
    return get_contact_parameters().get(0).getDissipation();
};
void HuntCrossleyForce::setDissipation(double dissipation) {
        if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
        upd_contact_parameters()[0].setDissipation(dissipation);
};
double HuntCrossleyForce::getStaticFriction()  {
    if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
    return get_contact_parameters().get(0).getStaticFriction();
};
void HuntCrossleyForce::setStaticFriction(double friction) {
        if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
        upd_contact_parameters()[0].setStaticFriction(friction);
};
double HuntCrossleyForce::getDynamicFriction()   {
    if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
    return get_contact_parameters().get(0).getDynamicFriction();
};
void HuntCrossleyForce::setDynamicFriction(double friction) {
        if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
        upd_contact_parameters()[0].setDynamicFriction(friction);
};
double HuntCrossleyForce::getViscousFriction()   {
    if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
    return get_contact_parameters().get(0).getViscousFriction();
};
void HuntCrossleyForce::setViscousFriction(double friction) {
        if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
        upd_contact_parameters()[0].setViscousFriction(friction);
};

void HuntCrossleyForce::addGeometry(const std::string& name)
{
    if (get_contact_parameters().getSize()==0)
            updContactParametersSet().adoptAndAppend(new HuntCrossleyForce::ContactParameters());
    upd_contact_parameters()[0].addGeometry(name);
}
//==============================================================================
//                HUNT CROSSLEY FORCE :: CONTACT PARAMETERS
//==============================================================================

// Default constructor.
HuntCrossleyForce::ContactParameters::ContactParameters()
{
    constructProperties();
}

// Constructor specifying material properties.
HuntCrossleyForce::ContactParameters::ContactParameters
   (double stiffness, double dissipation, double staticFriction,
    double dynamicFriction, double viscousFriction)
{
    constructProperties();
    set_stiffness(stiffness);
    set_dissipation(dissipation);
    set_static_friction(staticFriction);
    set_dynamic_friction(dynamicFriction);
    set_viscous_friction(viscousFriction);
}

void HuntCrossleyForce::ContactParameters::constructProperties()
{
    constructProperty_geometry(); // a list of strings
    constructProperty_stiffness(0.0);
    constructProperty_dissipation(0.0);
    constructProperty_static_friction(0.0);
    constructProperty_dynamic_friction(0.0);
    constructProperty_viscous_friction(0.0);
}

const Property<std::string>& HuntCrossleyForce::ContactParameters::getGeometry() const
{
    return getProperty_geometry();
}

Property<std::string>& HuntCrossleyForce::ContactParameters::updGeometry()
{
    return updProperty_geometry();
}

void HuntCrossleyForce::ContactParameters::addGeometry(const std::string& name)
{
    updGeometry().appendValue(name);
}

double HuntCrossleyForce::ContactParameters::getStiffness() const
{
    return get_stiffness();
}

void HuntCrossleyForce::ContactParameters::setStiffness(double stiffness)
{
    set_stiffness(stiffness);
}

double HuntCrossleyForce::ContactParameters::getDissipation() const
{
    return get_dissipation();
}

void HuntCrossleyForce::ContactParameters::setDissipation(double dissipation)
{
    set_dissipation(dissipation);
}

double HuntCrossleyForce::ContactParameters::getStaticFriction() const
{
    return get_static_friction();
}

void HuntCrossleyForce::ContactParameters::setStaticFriction(double friction)
{
    set_static_friction(friction);
}

double HuntCrossleyForce::ContactParameters::getDynamicFriction() const
{
    return get_dynamic_friction();
}

void HuntCrossleyForce::ContactParameters::setDynamicFriction(double friction)
{
    set_dynamic_friction(friction);
}

double HuntCrossleyForce::ContactParameters::getViscousFriction() const
{
    return get_viscous_friction();
}

void HuntCrossleyForce::ContactParameters::setViscousFriction(double friction)
{
    set_viscous_friction(friction);
}

void HuntCrossleyForce::ContactParametersSet::setNull()
{
    setAuthors("Peter Eastman");
}

HuntCrossleyForce::ContactParametersSet::ContactParametersSet()
{
    setNull();
}

//=============================================================================
// Reporting
//=============================================================================
/**
 * Provide names of the quantities (column labels) of the force value(s) reported
 *
 */
OpenSim::Array<std::string> HuntCrossleyForce::getRecordLabels() const
{
    OpenSim::Array<std::string> labels("");

    const ContactParametersSet& contactParametersSet =
        get_contact_parameters();

    for (int i = 0; i < contactParametersSet.getSize(); ++i)
    {
        ContactParameters& params = contactParametersSet.get(i);
        for (int j = 0; j < params.getGeometry().size(); ++j)
        {
            ContactGeometry& geom = _model->updContactGeometrySet().get(params.getGeometry()[j]);
            std::string bodyName = geom.getBodyName();
            labels.append(getName()+"."+bodyName+".force.X");
            labels.append(getName()+"."+bodyName+".force.Y");
            labels.append(getName()+"."+bodyName+".force.Z");
            labels.append(getName()+"."+bodyName+".torque.X");
            labels.append(getName()+"."+bodyName+".torque.Y");
            labels.append(getName()+"."+bodyName+".torque.Z");
        }
    }

    return labels;
}
/**
 * Provide the value(s) to be reported that correspond to the labels
 */
OpenSim::Array<double> HuntCrossleyForce::
getRecordValues(const SimTK::State& state) const
{
    OpenSim::Array<double> values(1);

    const ContactParametersSet& contactParametersSet =
        get_contact_parameters();

    const SimTK::HuntCrossleyForce& simtkForce =
        (SimTK::HuntCrossleyForce &)(_model->getForceSubsystem().getForce(_index));

    SimTK::Vector_<SimTK::SpatialVec> bodyForces(0);
    SimTK::Vector_<SimTK::Vec3> particleForces(0);
    SimTK::Vector mobilityForces(0);

    //get the net force added to the system contributed by the Spring
    simtkForce.calcForceContribution(state, bodyForces, particleForces,
                                     mobilityForces);

    for (int i = 0; i < contactParametersSet.getSize(); ++i)
    {
        ContactParameters& params = contactParametersSet.get(i);
        for (int j = 0; j < params.getGeometry().size(); ++j)
        {
            ContactGeometry& geom =
                _model->updContactGeometrySet().get(params.getGeometry()[j]);
            std::string bodyName = geom.getBodyName();

            SimTK::Vec3 forces =
                bodyForces(geom.getBody().getMobilizedBodyIndex())[1];
            SimTK::Vec3 torques =
                bodyForces(geom.getBody().getMobilizedBodyIndex())[0];

            values.append(3, &forces[0]);
            values.append(3, &torques[0]);
        }
    }

    return values;
}

}// end of namespace OpenSim
<|endoftext|>


<commit_before>/*      _______   __   __   __   ______   __   __   _______   __   __
 *     / _____/\ / /\ / /\ / /\ / ____/\ / /\ / /\ / ___  /\ /  |\/ /\
 *    / /\____\// / // / // / // /\___\// /_// / // /\_/ / // , |/ / /
 *   / / /__   / / // / // / // / /    / ___  / // ___  / // /| ' / /
 *  / /_// /\ / /_// / // / // /_/_   / / // / // /\_/ / // / |  / /
 * /______/ //______/ //_/ //_____/\ /_/ //_/ //_/ //_/ //_/ /|_/ /
 * \______\/ \______\/ \_\/ \_____\/ \_\/ \_\/ \_\/ \_\/ \_\/ \_\/
 *
 * Copyright (c) 2004, 2005 darkbits                        Js_./
 * Per Larsson a.k.a finalman                          _RqZ{a<^_aa
 * Olof Naessn a.k.a jansem/yakslem                _asww7!uY`>  )\a//
 *                                                 _Qhm`] _f "'c  1!5m
 * Visit: http://guichan.darkbits.org             )Qk<P ` _: :+' .'  "{[
 *                                               .)j(] .d_/ '-(  P .   S
 * License: (BSD)                                <Td/Z <fP"5(\"??"\a.  .L
 * Redistribution and use in source and          _dV>ws?a-?'      ._/L  #'
 * binary forms, with or without                 )4d[#7r, .   '     )d`)[
 * modification, are permitted provided         _Q-5'5W..j/?'   -?!\)cam'
 * that the following conditions are met:       j<<WP+k/);.        _W=j f
 * 1. Redistributions of source code must       .$%w\/]Q  . ."'  .  mj$
 *    retain the above copyright notice,        ]E.pYY(Q]>.   a     J@\
 *    this list of conditions and the           j(]1u<sE"L,. .   ./^ ]{a
 *    following disclaimer.                     4'_uomm\.  )L);-4     (3=
 * 2. Redistributions in binary form must        )_]X{Z('a_"a7'<a"a,  ]"[
 *    reproduce the above copyright notice,       #}<]m7`Za??4,P-"'7. ).m
 *    this list of conditions and the            ]d2e)Q(<Q(  ?94   b-  LQ/
 *    following disclaimer in the                <B!</]C)d_, '(<' .f. =C+m
 *    documentation and/or other materials      .Z!=J ]e []('-4f _ ) -.)m]'
 *    provided with the distribution.          .w[5]' _[ /.)_-"+?   _/ <W"
 * 3. Neither the name of Guichan nor the      :$we` _! + _/ .        j?
 *    names of its contributors may be used     =3)= _f  (_yQmWW$#(    "
 *    to endorse or promote products derived     -   W,  sQQQQmZQ#Wwa]..
 *    from this software without specific        (js, \[QQW$QWW#?!V"".
 *    prior written permission.                    ]y:.<\..          .
 *                                                 -]n w/ '         [.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT       )/ )/           !
 * HOLDERS AND CONTRIBUTORS "AS IS" AND ANY         <  (; sac    ,    '
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING,               ]^ .-  %
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF            c <   r
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR            aga<  <La
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE          5%  )P'-3L
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR        _bQf` y`..)a
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,          ,J?4P'.P"_(\?d'.,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES               _Pa,)!f/<[]/  ?"
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT      _2-..:. .r+_,.. .
 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,     ?a.<%"'  " -'.a_ _,
 * 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.
 */

/*
 * For comments regarding functions please see the header file.
 */

#include "guichan/opengl/openglgraphics.hpp"

#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#endif

#ifdef __amigaos4__
#include <mgl/gl.h>
#define glVertex3i glVertex3f
#else
#include <GL/gl.h>
#endif

#include "guichan/exception.hpp"
#include "guichan/image.hpp"
#include "guichan/opengl/openglimage.hpp"

namespace gcn
{
    OpenGLGraphics::OpenGLGraphics()
    {
        setTargetPlane(640, 480);
        mAlpha = false;
    }

    OpenGLGraphics::OpenGLGraphics(int width, int height)
    {
        setTargetPlane(width, height);
    }

    OpenGLGraphics::~OpenGLGraphics()
    {

    }

    void OpenGLGraphics::_beginDraw()
    {
        glPushAttrib(
            GL_COLOR_BUFFER_BIT |
            GL_CURRENT_BIT |
            GL_DEPTH_BUFFER_BIT |
            GL_ENABLE_BIT |
            GL_FOG_BIT |
            GL_LIGHTING_BIT |
            GL_LINE_BIT |
            GL_POINT_BIT |
            GL_POLYGON_BIT |
            GL_SCISSOR_BIT |
            GL_STENCIL_BUFFER_BIT |
            GL_TEXTURE_BIT |
            GL_TRANSFORM_BIT |
            GL_POINT_BIT |
            GL_LINE_BIT
            );

        glMatrixMode(GL_MODELVIEW);
        glPushMatrix();
        glLoadIdentity();

        glMatrixMode(GL_TEXTURE);
        glPushMatrix();
        glLoadIdentity();

        glMatrixMode(GL_PROJECTION);
        glPushMatrix();
        glLoadIdentity();

        glOrtho(0.0, (double)mWidth, (double)mHeight, 0.0, -1.0, 1.0);

        glDisable(GL_LIGHTING);
        glDisable(GL_CULL_FACE);
        glDisable(GL_DEPTH_TEST);
        glDisable(GL_TEXTURE_2D);

        glEnable(GL_SCISSOR_TEST);
        glPointSize(1.0);
        glLineWidth(1.0);

        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);

        glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);

        pushClipArea(Rectangle(0, 0, mWidth, mHeight));
    }

    void OpenGLGraphics::_endDraw()
    {
        glMatrixMode(GL_MODELVIEW);
        glPopMatrix();

        glMatrixMode(GL_TEXTURE);
        glPopMatrix();

        glMatrixMode(GL_PROJECTION);
        glPopMatrix();

        glPopAttrib();

        popClipArea();
    }

    bool OpenGLGraphics::pushClipArea(Rectangle area)
    {
        bool result = Graphics::pushClipArea(area);

        glScissor(mClipStack.top().x,
                  mHeight - mClipStack.top().y - mClipStack.top().height,
                  mClipStack.top().width,
                  mClipStack.top().height);

        return result;
    }

    void OpenGLGraphics::popClipArea()
    {
        Graphics::popClipArea();

        if (mClipStack.empty())
        {
            return;
        }

        glScissor(mClipStack.top().x,
                  mHeight - mClipStack.top().y - mClipStack.top().height,
                  mClipStack.top().width,
                  mClipStack.top().height);
    }

    void OpenGLGraphics::setTargetPlane(int width, int height)
    {
        mWidth = width;
        mHeight = height;
    }

    void OpenGLGraphics::drawImage(const Image* image, int srcX, int srcY,
                                   int dstX, int dstY, int width,
                                   int height)
    {
		const OpenGLImage* srcImage = dynamic_cast<const OpenGLImage*>(image);

        if (srcImage == NULL)
        {
            throw GCN_EXCEPTION("Trying to draw an image of unknown format, must be an OpenGLImage.");
        }

        dstX += mClipStack.top().xOffset;
        dstY += mClipStack.top().yOffset;

        // Find OpenGL texture coordinates
        float texX1 = srcX / (float)srcImage->getTextureWidth();
        float texY1 = srcY / (float)srcImage->getTextureHeight();
        float texX2 = (srcX+width) / (float)srcImage->getTextureWidth();
        float texY2 = (srcY+height) / (float)srcImage->getTextureHeight();

        glBindTexture(GL_TEXTURE_2D, srcImage->getTextureHandle());

        glEnable(GL_TEXTURE_2D);

        // Check if blending already is enabled
        if (!mAlpha)
        {
            glEnable(GL_BLEND);
        }

        // Draw a textured quad -- the image
        glBegin(GL_QUADS);
        glTexCoord2f(texX1, texY1);
        glVertex3i(dstX, dstY, 0);

        glTexCoord2f(texX1, texY2);
        glVertex3i(dstX, dstY + height, 0);

        glTexCoord2f(texX2, texY2);
        glVertex3i(dstX + width, dstY + height, 0);

        glTexCoord2f(texX2, texY1);
        glVertex3i(dstX + width, dstY, 0);
        glEnd();
        glDisable(GL_TEXTURE_2D);

        // Don't disable blending if the color has alpha
        if (!mAlpha)
        {
            glDisable(GL_BLEND);
        }
    }

    void OpenGLGraphics::drawPoint(int x, int y)
    {
        x += mClipStack.top().xOffset;
        y += mClipStack.top().yOffset;

        glBegin(GL_POINTS);
        glVertex3i(x, y, 0);
        glEnd();
    }

    void OpenGLGraphics::drawLine(int x1, int y1, int x2, int y2)
    {
        x1 += mClipStack.top().xOffset;
        y1 += mClipStack.top().yOffset;
        x2 += mClipStack.top().xOffset;
        y2 += mClipStack.top().yOffset;

        glBegin(GL_LINES);
        glVertex3f(x1+0.5f, y1+0.5f, 0);
        glVertex3f(x2+0.5f, y2+0.5f, 0);
        glEnd();

        glBegin(GL_POINTS);
        glVertex3f(x2+0.5f, y2+0.5f, 0);
        glEnd();
    }

    void OpenGLGraphics::drawRectangle(const Rectangle& rectangle)
    {
        glBegin(GL_LINE_LOOP);
        glVertex3f(rectangle.x + mClipStack.top().xOffset + 0.5f,
                   rectangle.y + mClipStack.top().yOffset + 0.5f, 0);
        glVertex3f(rectangle.x + rectangle.width - 0.5f + mClipStack.top().xOffset,
                   rectangle.y + mClipStack.top().yOffset + 0.5f, 0);
        glVertex3f(rectangle.x + rectangle.width - 0.5f + mClipStack.top().xOffset,
                   rectangle.y + rectangle.height + mClipStack.top().yOffset - 0.5f, 0);
        glVertex3f(rectangle.x + mClipStack.top().xOffset + 0.5f,
                   rectangle.y + rectangle.height + mClipStack.top().yOffset - 0.5f, 0);
        glEnd();
    }

    void OpenGLGraphics::fillRectangle(const Rectangle& rectangle)
    {
        glBegin(GL_QUADS);
        glVertex3i(rectangle.x + mClipStack.top().xOffset,
                   rectangle.y + mClipStack.top().yOffset, 0);
        glVertex3i(rectangle.x + rectangle.width + mClipStack.top().xOffset,
                   rectangle.y + mClipStack.top().yOffset, 0);
        glVertex3i(rectangle.x + rectangle.width + mClipStack.top().xOffset,
                   rectangle.y + rectangle.height + mClipStack.top().yOffset, 0);
        glVertex3i(rectangle.x + mClipStack.top().xOffset,
                   rectangle.y + rectangle.height + mClipStack.top().yOffset, 0);
        glEnd();
    }

    void OpenGLGraphics::setColor(const Color& color)
    {
        mColor = color;
        glColor4f(color.r/255.0,
                  color.g/255.0,
                  color.b/255.0,
                  color.a/255.0);

        mAlpha = color.a != 255;

        if (mAlpha)
        {
            glEnable(GL_BLEND);
        }
    }

    const Color& OpenGLGraphics::getColor()
    {
        return mColor;
    }
}
<commit_msg>OpenGLGraphics now uses glColor4ub in setColor.<commit_after>/*      _______   __   __   __   ______   __   __   _______   __   __
 *     / _____/\ / /\ / /\ / /\ / ____/\ / /\ / /\ / ___  /\ /  |\/ /\
 *    / /\____\// / // / // / // /\___\// /_// / // /\_/ / // , |/ / /
 *   / / /__   / / // / // / // / /    / ___  / // ___  / // /| ' / /
 *  / /_// /\ / /_// / // / // /_/_   / / // / // /\_/ / // / |  / /
 * /______/ //______/ //_/ //_____/\ /_/ //_/ //_/ //_/ //_/ /|_/ /
 * \______\/ \______\/ \_\/ \_____\/ \_\/ \_\/ \_\/ \_\/ \_\/ \_\/
 *
 * Copyright (c) 2004, 2005 darkbits                        Js_./
 * Per Larsson a.k.a finalman                          _RqZ{a<^_aa
 * Olof Naessn a.k.a jansem/yakslem                _asww7!uY`>  )\a//
 *                                                 _Qhm`] _f "'c  1!5m
 * Visit: http://guichan.darkbits.org             )Qk<P ` _: :+' .'  "{[
 *                                               .)j(] .d_/ '-(  P .   S
 * License: (BSD)                                <Td/Z <fP"5(\"??"\a.  .L
 * Redistribution and use in source and          _dV>ws?a-?'      ._/L  #'
 * binary forms, with or without                 )4d[#7r, .   '     )d`)[
 * modification, are permitted provided         _Q-5'5W..j/?'   -?!\)cam'
 * that the following conditions are met:       j<<WP+k/);.        _W=j f
 * 1. Redistributions of source code must       .$%w\/]Q  . ."'  .  mj$
 *    retain the above copyright notice,        ]E.pYY(Q]>.   a     J@\
 *    this list of conditions and the           j(]1u<sE"L,. .   ./^ ]{a
 *    following disclaimer.                     4'_uomm\.  )L);-4     (3=
 * 2. Redistributions in binary form must        )_]X{Z('a_"a7'<a"a,  ]"[
 *    reproduce the above copyright notice,       #}<]m7`Za??4,P-"'7. ).m
 *    this list of conditions and the            ]d2e)Q(<Q(  ?94   b-  LQ/
 *    following disclaimer in the                <B!</]C)d_, '(<' .f. =C+m
 *    documentation and/or other materials      .Z!=J ]e []('-4f _ ) -.)m]'
 *    provided with the distribution.          .w[5]' _[ /.)_-"+?   _/ <W"
 * 3. Neither the name of Guichan nor the      :$we` _! + _/ .        j?
 *    names of its contributors may be used     =3)= _f  (_yQmWW$#(    "
 *    to endorse or promote products derived     -   W,  sQQQQmZQ#Wwa]..
 *    from this software without specific        (js, \[QQW$QWW#?!V"".
 *    prior written permission.                    ]y:.<\..          .
 *                                                 -]n w/ '         [.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT       )/ )/           !
 * HOLDERS AND CONTRIBUTORS "AS IS" AND ANY         <  (; sac    ,    '
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING,               ]^ .-  %
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF            c <   r
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR            aga<  <La
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE          5%  )P'-3L
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR        _bQf` y`..)a
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,          ,J?4P'.P"_(\?d'.,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES               _Pa,)!f/<[]/  ?"
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT      _2-..:. .r+_,.. .
 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,     ?a.<%"'  " -'.a_ _,
 * 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.
 */

/*
 * For comments regarding functions please see the header file.
 */

#include "guichan/opengl/openglgraphics.hpp"

#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#endif

#ifdef __amigaos4__
#include <mgl/gl.h>
#define glVertex3i glVertex3f
#else
#include <GL/gl.h>
#endif

#include "guichan/exception.hpp"
#include "guichan/image.hpp"
#include "guichan/opengl/openglimage.hpp"

namespace gcn
{
    OpenGLGraphics::OpenGLGraphics()
    {
        setTargetPlane(640, 480);
        mAlpha = false;
    }

    OpenGLGraphics::OpenGLGraphics(int width, int height)
    {
        setTargetPlane(width, height);
    }

    OpenGLGraphics::~OpenGLGraphics()
    {

    }

    void OpenGLGraphics::_beginDraw()
    {
        glPushAttrib(
            GL_COLOR_BUFFER_BIT |
            GL_CURRENT_BIT |
            GL_DEPTH_BUFFER_BIT |
            GL_ENABLE_BIT |
            GL_FOG_BIT |
            GL_LIGHTING_BIT |
            GL_LINE_BIT |
            GL_POINT_BIT |
            GL_POLYGON_BIT |
            GL_SCISSOR_BIT |
            GL_STENCIL_BUFFER_BIT |
            GL_TEXTURE_BIT |
            GL_TRANSFORM_BIT |
            GL_POINT_BIT |
            GL_LINE_BIT
            );

        glMatrixMode(GL_MODELVIEW);
        glPushMatrix();
        glLoadIdentity();

        glMatrixMode(GL_TEXTURE);
        glPushMatrix();
        glLoadIdentity();

        glMatrixMode(GL_PROJECTION);
        glPushMatrix();
        glLoadIdentity();

        glOrtho(0.0, (double)mWidth, (double)mHeight, 0.0, -1.0, 1.0);

        glDisable(GL_LIGHTING);
        glDisable(GL_CULL_FACE);
        glDisable(GL_DEPTH_TEST);
        glDisable(GL_TEXTURE_2D);

        glEnable(GL_SCISSOR_TEST);
        glPointSize(1.0);
        glLineWidth(1.0);

        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);

        glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);

        pushClipArea(Rectangle(0, 0, mWidth, mHeight));
    }

    void OpenGLGraphics::_endDraw()
    {
        glMatrixMode(GL_MODELVIEW);
        glPopMatrix();

        glMatrixMode(GL_TEXTURE);
        glPopMatrix();

        glMatrixMode(GL_PROJECTION);
        glPopMatrix();

        glPopAttrib();

        popClipArea();
    }

    bool OpenGLGraphics::pushClipArea(Rectangle area)
    {
        bool result = Graphics::pushClipArea(area);

        glScissor(mClipStack.top().x,
                  mHeight - mClipStack.top().y - mClipStack.top().height,
                  mClipStack.top().width,
                  mClipStack.top().height);

        return result;
    }

    void OpenGLGraphics::popClipArea()
    {
        Graphics::popClipArea();

        if (mClipStack.empty())
        {
            return;
        }

        glScissor(mClipStack.top().x,
                  mHeight - mClipStack.top().y - mClipStack.top().height,
                  mClipStack.top().width,
                  mClipStack.top().height);
    }

    void OpenGLGraphics::setTargetPlane(int width, int height)
    {
        mWidth = width;
        mHeight = height;
    }

    void OpenGLGraphics::drawImage(const Image* image, int srcX, int srcY,
                                   int dstX, int dstY, int width,
                                   int height)
    {
		const OpenGLImage* srcImage = dynamic_cast<const OpenGLImage*>(image);

        if (srcImage == NULL)
        {
            throw GCN_EXCEPTION("Trying to draw an image of unknown format, must be an OpenGLImage.");
        }

        dstX += mClipStack.top().xOffset;
        dstY += mClipStack.top().yOffset;

        // Find OpenGL texture coordinates
        float texX1 = srcX / (float)srcImage->getTextureWidth();
        float texY1 = srcY / (float)srcImage->getTextureHeight();
        float texX2 = (srcX+width) / (float)srcImage->getTextureWidth();
        float texY2 = (srcY+height) / (float)srcImage->getTextureHeight();

        glBindTexture(GL_TEXTURE_2D, srcImage->getTextureHandle());

        glEnable(GL_TEXTURE_2D);

        // Check if blending already is enabled
        if (!mAlpha)
        {
            glEnable(GL_BLEND);
        }

        // Draw a textured quad -- the image
        glBegin(GL_QUADS);
        glTexCoord2f(texX1, texY1);
        glVertex3i(dstX, dstY, 0);

        glTexCoord2f(texX1, texY2);
        glVertex3i(dstX, dstY + height, 0);

        glTexCoord2f(texX2, texY2);
        glVertex3i(dstX + width, dstY + height, 0);

        glTexCoord2f(texX2, texY1);
        glVertex3i(dstX + width, dstY, 0);
        glEnd();
        glDisable(GL_TEXTURE_2D);

        // Don't disable blending if the color has alpha
        if (!mAlpha)
        {
            glDisable(GL_BLEND);
        }
    }

    void OpenGLGraphics::drawPoint(int x, int y)
    {
        x += mClipStack.top().xOffset;
        y += mClipStack.top().yOffset;

        glBegin(GL_POINTS);
        glVertex3i(x, y, 0);
        glEnd();
    }

    void OpenGLGraphics::drawLine(int x1, int y1, int x2, int y2)
    {
        x1 += mClipStack.top().xOffset;
        y1 += mClipStack.top().yOffset;
        x2 += mClipStack.top().xOffset;
        y2 += mClipStack.top().yOffset;

        glBegin(GL_LINES);
        glVertex3f(x1+0.5f, y1+0.5f, 0);
        glVertex3f(x2+0.5f, y2+0.5f, 0);
        glEnd();

        glBegin(GL_POINTS);
        glVertex3f(x2+0.5f, y2+0.5f, 0);
        glEnd();
    }

    void OpenGLGraphics::drawRectangle(const Rectangle& rectangle)
    {
        glBegin(GL_LINE_LOOP);
        glVertex3f(rectangle.x + mClipStack.top().xOffset + 0.5f,
                   rectangle.y + mClipStack.top().yOffset + 0.5f, 0);
        glVertex3f(rectangle.x + rectangle.width - 0.5f + mClipStack.top().xOffset,
                   rectangle.y + mClipStack.top().yOffset + 0.5f, 0);
        glVertex3f(rectangle.x + rectangle.width - 0.5f + mClipStack.top().xOffset,
                   rectangle.y + rectangle.height + mClipStack.top().yOffset - 0.5f, 0);
        glVertex3f(rectangle.x + mClipStack.top().xOffset + 0.5f,
                   rectangle.y + rectangle.height + mClipStack.top().yOffset - 0.5f, 0);
        glEnd();
    }

    void OpenGLGraphics::fillRectangle(const Rectangle& rectangle)
    {
        glBegin(GL_QUADS);
        glVertex3i(rectangle.x + mClipStack.top().xOffset,
                   rectangle.y + mClipStack.top().yOffset, 0);
        glVertex3i(rectangle.x + rectangle.width + mClipStack.top().xOffset,
                   rectangle.y + mClipStack.top().yOffset, 0);
        glVertex3i(rectangle.x + rectangle.width + mClipStack.top().xOffset,
                   rectangle.y + rectangle.height + mClipStack.top().yOffset, 0);
        glVertex3i(rectangle.x + mClipStack.top().xOffset,
                   rectangle.y + rectangle.height + mClipStack.top().yOffset, 0);
        glEnd();
    }

    void OpenGLGraphics::setColor(const Color& color)
    {
        mColor = color;
        glColor4ub(color.r, color.g, color.b, color.a);

        mAlpha = color.a != 255;

        if (mAlpha)
        {
            glEnable(GL_BLEND);
        }
    }

    const Color& OpenGLGraphics::getColor()
    {
        return mColor;
    }
}
<|endoftext|>


<commit_before>#include "box2dmousejoint.h"
#include "box2dbody.h"
#include "box2dworld.h"

Box2DMouseJoint::Box2DMouseJoint(QDeclarativeItem *parent) :
    Box2DJoint(parent),
    mMouseJointDef(),
    mMouseJoint(0)
{
}

float Box2DMouseJoint::maxForce() const
{
    return mMouseJointDef.maxForce;
}

void Box2DMouseJoint::setMaxForce(float force)
{
    if (mMouseJointDef.maxForce == force)
        return;

    mMouseJointDef.maxForce = force;
    if (mMouseJoint)
        mMouseJoint->SetMaxForce(force);
    emit maxForceChanged();
}

float Box2DMouseJoint::frequencyHz() const
{
    return mMouseJointDef.frequencyHz;
}

void Box2DMouseJoint::setFrequencyHz(float frequency)
{
    if (mMouseJointDef.frequencyHz == frequency)
        return;

    mMouseJointDef.frequencyHz = frequency;
    if (mMouseJoint)
        mMouseJoint->SetFrequency(frequency);
    emit frequencyHzChanged();
}

float Box2DMouseJoint::dampingRatio() const
{
    return mMouseJointDef.dampingRatio;
}

void Box2DMouseJoint::setDampingRatio(float damping)
{
    if (mMouseJointDef.dampingRatio == damping)
        return;

    mMouseJointDef.dampingRatio = damping;
    if (mMouseJoint)
        mMouseJoint->SetDampingRatio(damping);
    emit dampingRatioChanged();
}

QPointF const Box2DMouseJoint::reactionForce(float inv)
{
    const b2Vec2 rf = mMouseJoint->GetReactionForce(inv);

    return QPointF(rf.x, rf.y);
}

float Box2DMouseJoint::reactionTorque(float inv)
{
    return mMouseJoint->GetReactionTorque(inv);
}

void Box2DMouseJoint::setTarget(const QPointF &targetxy)
{
    qDebug() << "TXY: " << targetxy;
    mMouseJoint->SetTarget(b2Vec2(targetxy.x() / scaleRatio, -targetxy.y() / scaleRatio));
}

QPointF const Box2DMouseJoint::getTarget()
{
    const b2Vec2 rf = mMouseJoint->GetTarget();

    return QPointF(rf.x, rf.y);
}

void Box2DMouseJoint::createJoint()
{
    mMouseJointDef.collideConnected = collideConnected();
    mMouseJointDef.bodyA = bodyA()->body();
    mMouseJointDef.bodyB = bodyB()->body();
    mMouseJointDef.target = b2Vec2(x() / scaleRatio, -y() / scaleRatio);

    mMouseJoint = static_cast<b2MouseJoint*>(world()->CreateJoint(&mMouseJointDef));
    mMouseJointDef.bodyB->SetAwake(true);
    mInitializePending = false;
}

void Box2DMouseJoint::nullifyJoint()
{
    targetIsValid=false;
    mMouseJoint = 0;
}

void Box2DMouseJoint::cleanup(b2World *world)
{
    if (mMouseJoint && bodyA() && bodyB()) {
        mMouseJoint->SetUserData(0);
        world->DestroyJoint(mMouseJoint);
        mMouseJoint = 0;
    }
}
<commit_msg>MouseJoint: Check that we have a mouseJoint in SetTarget() and if not update the target in the joint definition<commit_after>#include "box2dmousejoint.h"
#include "box2dbody.h"
#include "box2dworld.h"

Box2DMouseJoint::Box2DMouseJoint(QDeclarativeItem *parent) :
    Box2DJoint(parent),
    mMouseJointDef(),
    mMouseJoint(0)
{
}

float Box2DMouseJoint::maxForce() const
{
    return mMouseJointDef.maxForce;
}

void Box2DMouseJoint::setMaxForce(float force)
{
    if (mMouseJointDef.maxForce == force)
        return;

    mMouseJointDef.maxForce = force;
    if (mMouseJoint)
        mMouseJoint->SetMaxForce(force);
    emit maxForceChanged();
}

float Box2DMouseJoint::frequencyHz() const
{
    return mMouseJointDef.frequencyHz;
}

void Box2DMouseJoint::setFrequencyHz(float frequency)
{
    if (mMouseJointDef.frequencyHz == frequency)
        return;

    mMouseJointDef.frequencyHz = frequency;
    if (mMouseJoint)
        mMouseJoint->SetFrequency(frequency);
    emit frequencyHzChanged();
}

float Box2DMouseJoint::dampingRatio() const
{
    return mMouseJointDef.dampingRatio;
}

void Box2DMouseJoint::setDampingRatio(float damping)
{
    if (mMouseJointDef.dampingRatio == damping)
        return;

    mMouseJointDef.dampingRatio = damping;
    if (mMouseJoint)
        mMouseJoint->SetDampingRatio(damping);
    emit dampingRatioChanged();
}

QPointF const Box2DMouseJoint::reactionForce(float inv)
{
    const b2Vec2 rf = mMouseJoint->GetReactionForce(inv);

    return QPointF(rf.x, rf.y);
}

float Box2DMouseJoint::reactionTorque(float inv)
{
    return mMouseJoint->GetReactionTorque(inv);
}

void Box2DMouseJoint::setTarget(const QPointF &targetxy)
{
    if (mMouseJoint) {
        mMouseJoint->SetTarget(b2Vec2(targetxy.x() / scaleRatio, -targetxy.y() / scaleRatio));
    } else {
        mMouseJointDef.target = b2Vec2(x() / scaleRatio, -y() / scaleRatio);
    }
}

QPointF const Box2DMouseJoint::getTarget()
{
    const b2Vec2 rf = mMouseJoint->GetTarget();

    return QPointF(rf.x, rf.y);
}

void Box2DMouseJoint::createJoint()
{
    mMouseJointDef.collideConnected = collideConnected();
    mMouseJointDef.bodyA = bodyA()->body();
    mMouseJointDef.bodyB = bodyB()->body();
    mMouseJointDef.target = b2Vec2(x() / scaleRatio, -y() / scaleRatio);

    mMouseJoint = static_cast<b2MouseJoint*>(world()->CreateJoint(&mMouseJointDef));
    mMouseJointDef.bodyB->SetAwake(true);
    mInitializePending = false;
}

void Box2DMouseJoint::nullifyJoint()
{
    targetIsValid=false;
    mMouseJoint = 0;
}

void Box2DMouseJoint::cleanup(b2World *world)
{
    if (mMouseJoint && bodyA() && bodyB()) {
        mMouseJoint->SetUserData(0);
        world->DestroyJoint(mMouseJoint);
        mMouseJoint = 0;
    }
}
<|endoftext|>


<commit_before>#include <b9/ExecutionContext.hpp>
#include <b9/VirtualMachine.hpp>
#include <b9/compiler/Compiler.hpp>
#include <b9/loader.hpp>

#include <omrgc.h>
#include <OMR/Om/Allocator.inl.hpp>
#include <OMR/Om/ArrayBuffer.inl.hpp>
#include <OMR/Om/ArrayBufferMap.inl.hpp>
#include <OMR/Om/Map.inl.hpp>
#include <OMR/Om/Object.inl.hpp>
#include <OMR/Om/ObjectMap.inl.hpp>
#include <OMR/Om/RootRef.inl.hpp>
#include <OMR/Om/Value.hpp>
#include "Jit.hpp"

#include <sys/time.h>
#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <sstream>
#include <string>

namespace b9 {

ExecutionContext::ExecutionContext(VirtualMachine &virtualMachine,
                                   const Config &cfg)
    : omContext_(virtualMachine.memoryManager()),
      virtualMachine_(&virtualMachine),
      cfg_(&cfg) {
  omContext().userRoots().push_back(
      [this](Om::Context &cx, Om::Visitor &v) { this->visit(cx, v); });
}

void ExecutionContext::reset() {
  stack_.reset();
  programCounter_ = 0;
}

StackElement ExecutionContext::interpret(const std::size_t functionIndex) {
  auto function = virtualMachine_->getFunction(functionIndex);
  auto argsCount = function->nargs;
  auto jitFunction = virtualMachine_->getJitAddress(functionIndex);

  if (jitFunction) {
    if (cfg_->debug) {
      std::cout << "Calling " << function << " jit: " << jitFunction
                << std::endl;
    }
    OMR::Om::RawValue result = 0;
    if (cfg_->passParam) {
      switch (argsCount) {
        case 0: {
          result = jitFunction(this);
        } break;
        case 1: {
          OMR::Om::RawValue p1 = pop().raw();
          result = jitFunction(this, p1);
        } break;
        case 2: {
          OMR::Om::RawValue p2 = pop().raw();
          OMR::Om::RawValue p1 = pop().raw();
          result = jitFunction(this, p1, p2);
        } break;
        case 3: {
          OMR::Om::RawValue p3 = pop().raw();
          OMR::Om::RawValue p2 = pop().raw();
          OMR::Om::RawValue p1 = pop().raw();
          result = (*jitFunction)(this, p1, p2, p3);
        } break;
        default:
          throw std::runtime_error{"Need to add handlers for more parameters"};
          break;
      }
    } else {
      // Call the Jit'ed function, passing the parameters on the
      // ExecutionContext stack.
      if (cfg_->debug) std::cout << "passing parameters on the stack\n";
      result = jitFunction(this);
    }
    return OMR::Om::Value(result);
  }

  // interpret the method otherwise
  const Instruction *instructionPointer = function->address;

  StackElement *args = stack_.top() - function->nargs;
  stack_.pushn(function->nregs);

  while (*instructionPointer != END_SECTION) {
    switch (instructionPointer->byteCode()) {
      case ByteCode::FUNCTION_CALL:
        doFunctionCall(instructionPointer->parameter());
        break;
      case ByteCode::FUNCTION_RETURN: {
        auto result = stack_.pop();
        stack_.restore(args);
        return result;
        break;
      }
      case ByteCode::PRIMITIVE_CALL:
        doPrimitiveCall(instructionPointer->parameter());
        break;
      case ByteCode::JMP:
        instructionPointer += instructionPointer->parameter();
        break;
      case ByteCode::DUPLICATE:
        // TODO
        break;
      case ByteCode::DROP:
        doDrop();
        break;
      case ByteCode::PUSH_FROM_VAR:
        doPushFromVar(args, instructionPointer->parameter());
        break;
      case ByteCode::POP_INTO_VAR:
        // TODO bad name, push or pop?
        doPushIntoVar(args, instructionPointer->parameter());
        break;
      case ByteCode::INT_ADD:
        doIntAdd();
        break;
      case ByteCode::INT_SUB:
        doIntSub();
        break;
      case ByteCode::INT_PUSH_CONSTANT:
        doIntPushConstant(instructionPointer->parameter());
        break;
      case ByteCode::INT_NOT:
        doIntNot();
        break;
      case ByteCode::INT_JMP_EQ:
        instructionPointer += doIntJmpEq(instructionPointer->parameter());
        break;
      case ByteCode::INT_JMP_NEQ:
        instructionPointer += doIntJmpNeq(instructionPointer->parameter());
        break;
      case ByteCode::INT_JMP_GT:
        instructionPointer += doIntJmpGt(instructionPointer->parameter());
        break;
      case ByteCode::INT_JMP_GE:
        instructionPointer += doIntJmpGe(instructionPointer->parameter());
        break;
      case ByteCode::INT_JMP_LT:
        instructionPointer += doIntJmpLt(instructionPointer->parameter());
        break;
      case ByteCode::INT_JMP_LE:
        instructionPointer += doIntJmpLe(instructionPointer->parameter());
        break;
      case ByteCode::STR_PUSH_CONSTANT:
        doStrPushConstant(instructionPointer->parameter());
        break;
      case ByteCode::STR_JMP_EQ:
        // TODO
        break;
      case ByteCode::STR_JMP_NEQ:
        // TODO
        break;
      case ByteCode::NEW_OBJECT:
        doNewObject();
        break;
      case ByteCode::PUSH_FROM_OBJECT:
        doPushFromObject(OMR::Om::Id(instructionPointer->parameter()));
        break;
      case ByteCode::POP_INTO_OBJECT:
        doPopIntoObject(OMR::Om::Id(instructionPointer->parameter()));
        break;
      case ByteCode::CALL_INDIRECT:
        doCallIndirect();
        break;
      case ByteCode::SYSTEM_COLLECT:
        doSystemCollect();
        break;
      default:
        assert(false);
        break;
    }
    instructionPointer++;
    programCounter_++;
  }
  throw std::runtime_error("Reached end of function");
}

void ExecutionContext::push(StackElement value) { stack_.push(value); }

StackElement ExecutionContext::pop() { return stack_.pop(); }

void ExecutionContext::doFunctionCall(Parameter value) {
  auto f = virtualMachine_->getFunction((std::size_t)value);
  auto result = interpret(value);
  push(result);
}

void ExecutionContext::doFunctionReturn(StackElement returnVal) {
  // TODO
}

void ExecutionContext::doPrimitiveCall(Parameter value) {
  PrimitiveFunction *primitive = virtualMachine_->getPrimitive(value);
  (*primitive)(this);
}

Parameter ExecutionContext::doJmp(Parameter offset) { return offset; }

void ExecutionContext::doDuplicate() {
  // TODO
}

void ExecutionContext::doDrop() { stack_.pop(); }

void ExecutionContext::doPushFromVar(StackElement *args, Parameter offset) {
  stack_.push(args[offset]);
}

void ExecutionContext::doPushIntoVar(StackElement *args, Parameter offset) {
  args[offset] = stack_.pop();
}

void ExecutionContext::doIntAdd() {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  StackElement result;
  result.setInteger(left + right);
  push(result);
}

void ExecutionContext::doIntSub() {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  StackElement result;
  result.setInteger(left - right);
  push(result);
}

void ExecutionContext::doIntPushConstant(Parameter value) {
  stack_.push(StackElement().setInteger(value));
}

void ExecutionContext::doIntNot() {
  std::int32_t i = stack_.pop().getInteger();
  StackElement v;
  v.setInteger(!i);
  push(v);
}

Parameter ExecutionContext::doIntJmpEq(Parameter delta) {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  if (left == right) {
    return delta;
  }
  return 0;
}

Parameter ExecutionContext::doIntJmpNeq(Parameter delta) {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  if (left != right) {
    return delta;
  }
  return 0;
}

Parameter ExecutionContext::doIntJmpGt(Parameter delta) {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  if (left > right) {
    return delta;
  }
  return 0;
}

// ( left right -- )
Parameter ExecutionContext::doIntJmpGe(Parameter delta) {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  if (left >= right) {
    return delta;
  }
  return 0;
}

// ( left right -- )
Parameter ExecutionContext::doIntJmpLt(Parameter delta) {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  if (left < right) {
    return delta;
  }
  return 0;
}

// ( left right -- )
Parameter ExecutionContext::doIntJmpLe(Parameter delta) {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  if (left <= right) {
    return delta;
  }
  return 0;
}

// ( -- string )
void ExecutionContext::doStrPushConstant(Parameter param) {
  stack_.push(OMR::Om::Value().setInteger(param));
}

// ( -- object )
void ExecutionContext::doNewObject() {
  auto ref = OMR::Om::Object::allocate(*this);
  stack_.push(OMR::Om::Value(ref));
}

// ( object -- value )
void ExecutionContext::doPushFromObject(Om::Id slotId) {
  auto value = stack_.pop();
  if (!value.isPtr()) {
    throw std::runtime_error("Accessing non-object value as an object.");
  }
  auto obj = value.getPtr<Om::Object>();
  Om::SlotDescriptor descriptor;
  auto found = Om::Object::lookup(*this, obj, slotId, descriptor);
  if (found) {
    Om::Value result;
    result = Om::Object::getValue(*this, obj, descriptor);
    stack_.push(result);
  } else {
    throw std::runtime_error("Accessing an object's field that doesn't exist.");
  }
}

// ( object value -- )
void ExecutionContext::doPopIntoObject(Om::Id slotId) {
  if (!stack_.peek().isPtr()) {
    throw std::runtime_error("Accessing non-object as an object");
  }

  std::size_t offset = 0;
  auto object = stack_.pop().getPtr<Om::Object>();

  Om::SlotDescriptor descriptor;
  bool found = Om::Object::lookup(*this, object, slotId, descriptor);

  if (!found) {
    static constexpr Om::SlotType type(Om::Id(0), Om::CoreType::VALUE);

    Om::RootRef<Om::Object> root(*this, object);
    auto map = Om::Object::transition(*this, root, {{type, slotId}});
    assert(map != nullptr);

    // TODO: Get the descriptor fast after a single-slot transition.
    Om::Object::lookup(*this, object, slotId, descriptor);
    object = root.get();
  }

  auto val = pop();
  Om::Object::setValue(*this, object, descriptor, val);
  // TODO: Write barrier the object on store.
}

void ExecutionContext::doCallIndirect() {
  assert(0);  // TODO: Implement call indirect
}

void ExecutionContext::doSystemCollect() {
  std::cout << "SYSTEM COLLECT!!!" << std::endl;
  OMR_GC_SystemCollect(omContext_.omrVmThread(), 0);
}

}  // namespace b9
<commit_msg>TMP: Disable PP in the int2jit transition to fix a bug?<commit_after>#include <b9/ExecutionContext.hpp>
#include <b9/VirtualMachine.hpp>
#include <b9/compiler/Compiler.hpp>
#include <b9/loader.hpp>

#include <omrgc.h>
#include <OMR/Om/Allocator.inl.hpp>
#include <OMR/Om/ArrayBuffer.inl.hpp>
#include <OMR/Om/ArrayBufferMap.inl.hpp>
#include <OMR/Om/Map.inl.hpp>
#include <OMR/Om/Object.inl.hpp>
#include <OMR/Om/ObjectMap.inl.hpp>
#include <OMR/Om/RootRef.inl.hpp>
#include <OMR/Om/Value.hpp>
#include "Jit.hpp"

#include <sys/time.h>
#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <sstream>
#include <string>

namespace b9 {

ExecutionContext::ExecutionContext(VirtualMachine &virtualMachine,
                                   const Config &cfg)
    : omContext_(virtualMachine.memoryManager()),
      virtualMachine_(&virtualMachine),
      cfg_(&cfg) {
  omContext().userRoots().push_back(
      [this](Om::Context &cx, Om::Visitor &v) { this->visit(cx, v); });
}

void ExecutionContext::reset() {
  stack_.reset();
  programCounter_ = 0;
}

static Om::RawValue callJitFunction(JitFunction fn, ExecutionContext* executionContext) {
  return fn(executionContext);
}

StackElement ExecutionContext::interpret(const std::size_t functionIndex) {
  auto function = virtualMachine_->getFunction(functionIndex);
  auto argsCount = function->nargs;
  auto jitFunction = virtualMachine_->getJitAddress(functionIndex);

  if (jitFunction) {
    auto result = callJitFunction(jitFunction, this);
    return Om::Value(result);
  }

  // interpret the method otherwise
  const Instruction *instructionPointer = function->address;

  StackElement *args = stack_.top() - function->nargs;
  stack_.pushn(function->nregs);

  while (*instructionPointer != END_SECTION) {
    switch (instructionPointer->byteCode()) {
      case ByteCode::FUNCTION_CALL:
        doFunctionCall(instructionPointer->parameter());
        break;
      case ByteCode::FUNCTION_RETURN: {
        auto result = stack_.pop();
        stack_.restore(args);
        return result;
        break;
      }
      case ByteCode::PRIMITIVE_CALL:
        doPrimitiveCall(instructionPointer->parameter());
        break;
      case ByteCode::JMP:
        instructionPointer += instructionPointer->parameter();
        break;
      case ByteCode::DUPLICATE:
        // TODO
        break;
      case ByteCode::DROP:
        doDrop();
        break;
      case ByteCode::PUSH_FROM_VAR:
        doPushFromVar(args, instructionPointer->parameter());
        break;
      case ByteCode::POP_INTO_VAR:
        // TODO bad name, push or pop?
        doPushIntoVar(args, instructionPointer->parameter());
        break;
      case ByteCode::INT_ADD:
        doIntAdd();
        break;
      case ByteCode::INT_SUB:
        doIntSub();
        break;
      case ByteCode::INT_PUSH_CONSTANT:
        doIntPushConstant(instructionPointer->parameter());
        break;
      case ByteCode::INT_NOT:
        doIntNot();
        break;
      case ByteCode::INT_JMP_EQ:
        instructionPointer += doIntJmpEq(instructionPointer->parameter());
        break;
      case ByteCode::INT_JMP_NEQ:
        instructionPointer += doIntJmpNeq(instructionPointer->parameter());
        break;
      case ByteCode::INT_JMP_GT:
        instructionPointer += doIntJmpGt(instructionPointer->parameter());
        break;
      case ByteCode::INT_JMP_GE:
        instructionPointer += doIntJmpGe(instructionPointer->parameter());
        break;
      case ByteCode::INT_JMP_LT:
        instructionPointer += doIntJmpLt(instructionPointer->parameter());
        break;
      case ByteCode::INT_JMP_LE:
        instructionPointer += doIntJmpLe(instructionPointer->parameter());
        break;
      case ByteCode::STR_PUSH_CONSTANT:
        doStrPushConstant(instructionPointer->parameter());
        break;
      case ByteCode::STR_JMP_EQ:
        // TODO
        break;
      case ByteCode::STR_JMP_NEQ:
        // TODO
        break;
      case ByteCode::NEW_OBJECT:
        doNewObject();
        break;
      case ByteCode::PUSH_FROM_OBJECT:
        doPushFromObject(OMR::Om::Id(instructionPointer->parameter()));
        break;
      case ByteCode::POP_INTO_OBJECT:
        doPopIntoObject(OMR::Om::Id(instructionPointer->parameter()));
        break;
      case ByteCode::CALL_INDIRECT:
        doCallIndirect();
        break;
      case ByteCode::SYSTEM_COLLECT:
        doSystemCollect();
        break;
      default:
        assert(false);
        break;
    }
    instructionPointer++;
    programCounter_++;
  }
  throw std::runtime_error("Reached end of function");
}

void ExecutionContext::push(StackElement value) { stack_.push(value); }

StackElement ExecutionContext::pop() { return stack_.pop(); }

void ExecutionContext::doFunctionCall(Parameter value) {
  auto f = virtualMachine_->getFunction((std::size_t)value);
  auto result = interpret(value);
  push(result);
}

void ExecutionContext::doFunctionReturn(StackElement returnVal) {
  // TODO
}

void ExecutionContext::doPrimitiveCall(Parameter value) {
  PrimitiveFunction *primitive = virtualMachine_->getPrimitive(value);
  (*primitive)(this);
}

Parameter ExecutionContext::doJmp(Parameter offset) { return offset; }

void ExecutionContext::doDuplicate() {
  // TODO
}

void ExecutionContext::doDrop() { stack_.pop(); }

void ExecutionContext::doPushFromVar(StackElement *args, Parameter offset) {
  stack_.push(args[offset]);
}

void ExecutionContext::doPushIntoVar(StackElement *args, Parameter offset) {
  args[offset] = stack_.pop();
}

void ExecutionContext::doIntAdd() {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  StackElement result;
  result.setInteger(left + right);
  push(result);
}

void ExecutionContext::doIntSub() {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  StackElement result;
  result.setInteger(left - right);
  push(result);
}

void ExecutionContext::doIntPushConstant(Parameter value) {
  stack_.push(StackElement().setInteger(value));
}

void ExecutionContext::doIntNot() {
  std::int32_t i = stack_.pop().getInteger();
  StackElement v;
  v.setInteger(!i);
  push(v);
}

Parameter ExecutionContext::doIntJmpEq(Parameter delta) {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  if (left == right) {
    return delta;
  }
  return 0;
}

Parameter ExecutionContext::doIntJmpNeq(Parameter delta) {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  if (left != right) {
    return delta;
  }
  return 0;
}

Parameter ExecutionContext::doIntJmpGt(Parameter delta) {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  if (left > right) {
    return delta;
  }
  return 0;
}

// ( left right -- )
Parameter ExecutionContext::doIntJmpGe(Parameter delta) {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  if (left >= right) {
    return delta;
  }
  return 0;
}

// ( left right -- )
Parameter ExecutionContext::doIntJmpLt(Parameter delta) {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  if (left < right) {
    return delta;
  }
  return 0;
}

// ( left right -- )
Parameter ExecutionContext::doIntJmpLe(Parameter delta) {
  std::int32_t right = stack_.pop().getInteger();
  std::int32_t left = stack_.pop().getInteger();
  if (left <= right) {
    return delta;
  }
  return 0;
}

// ( -- string )
void ExecutionContext::doStrPushConstant(Parameter param) {
  stack_.push(OMR::Om::Value().setInteger(param));
}

// ( -- object )
void ExecutionContext::doNewObject() {
  auto ref = OMR::Om::Object::allocate(*this);
  stack_.push(OMR::Om::Value(ref));
}

// ( object -- value )
void ExecutionContext::doPushFromObject(Om::Id slotId) {
  auto value = stack_.pop();
  if (!value.isPtr()) {
    throw std::runtime_error("Accessing non-object value as an object.");
  }
  auto obj = value.getPtr<Om::Object>();
  Om::SlotDescriptor descriptor;
  auto found = Om::Object::lookup(*this, obj, slotId, descriptor);
  if (found) {
    Om::Value result;
    result = Om::Object::getValue(*this, obj, descriptor);
    stack_.push(result);
  } else {
    throw std::runtime_error("Accessing an object's field that doesn't exist.");
  }
}

// ( object value -- )
void ExecutionContext::doPopIntoObject(Om::Id slotId) {
  if (!stack_.peek().isPtr()) {
    throw std::runtime_error("Accessing non-object as an object");
  }

  std::size_t offset = 0;
  auto object = stack_.pop().getPtr<Om::Object>();

  Om::SlotDescriptor descriptor;
  bool found = Om::Object::lookup(*this, object, slotId, descriptor);

  if (!found) {
    static constexpr Om::SlotType type(Om::Id(0), Om::CoreType::VALUE);

    Om::RootRef<Om::Object> root(*this, object);
    auto map = Om::Object::transition(*this, root, {{type, slotId}});
    assert(map != nullptr);

    // TODO: Get the descriptor fast after a single-slot transition.
    Om::Object::lookup(*this, object, slotId, descriptor);
    object = root.get();
  }

  auto val = pop();
  Om::Object::setValue(*this, object, descriptor, val);
  // TODO: Write barrier the object on store.
}

void ExecutionContext::doCallIndirect() {
  assert(0);  // TODO: Implement call indirect
}

void ExecutionContext::doSystemCollect() {
  std::cout << "SYSTEM COLLECT!!!" << std::endl;
  OMR_GC_SystemCollect(omContext_.omrVmThread(), 0);
}

}  // namespace b9
<|endoftext|>