<commit_before>/*
 * Copyright (c) 2011, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s): Jeongseok Lee <jslee02@gmail.com>
 * Date: 05/23/2013
 *
 * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab
 *
 * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
 * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   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 HOLDER 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.
 */

#include <iostream>
#include <gtest/gtest.h>
#include "TestHelpers.h"

#include "math/Geometry.h"
#include "math/Helpers.h"
#include "dynamics/BallJoint.h"
#include "dynamics/FreeJoint.h"
#include "dynamics/PrismaticJoint.h"
#include "dynamics/RevoluteJoint.h"
#include "dynamics/Skeleton.h"
#include "dynamics/TranslationalJoint.h"
#include "dynamics/UniversalJoint.h"
#include "dynamics/WeldJoint.h"
#include "dynamics/EulerJoint.h"
#include "dynamics/ScrewJoint.h"
#include "dynamics/BodyNode.h"
#include "dynamics/Skeleton.h"
#include "simulation/World.h"
#include "utils/Paths.h"
#include "utils/SkelParser.h"

using namespace dart;
using namespace math;
using namespace dynamics;

#define JOINT_TOL 0.01

/******************************************************************************/
class JOINTS : public testing::Test
{
public:
    void kinematicsTest(Joint* _joint);
};

/******************************************************************************/
void JOINTS::kinematicsTest(Joint* _joint)
{
    BodyNode bodyNode;
    bodyNode.setParentJoint(_joint);

    Skeleton skeleton;
    skeleton.addBodyNode(&bodyNode);
    skeleton.init();

    int dof = _joint->getNumGenCoords();

    //--------------------------------------------------------------------------
    //
    //--------------------------------------------------------------------------
    VectorXd q = VectorXd::Zero(dof);
    VectorXd dq = VectorXd::Zero(dof);
    VectorXd ddq = VectorXd::Zero(dof);

    for (int idxTest = 0; idxTest < 100; ++idxTest)
    {
        double dt = 0.000001;

        for (int i = 0; i < dof; ++i)
        {
            q(i) = random(-DART_PI, DART_PI);
            dq(i) = random(-DART_PI, DART_PI);
            ddq(i) = random(-DART_PI, DART_PI);
        }

        _joint->set_q(q);
        _joint->set_dq(dq);
        _joint->set_ddq(ddq);

        bodyNode.updateTransform();
        bodyNode.updateVelocity();
        bodyNode.updateEta();
        bodyNode.updateAcceleration();

        if (_joint->getNumGenCoords() == 0)
            return;

        Eigen::Isometry3d T = _joint->getLocalTransform();
        Jacobian J = _joint->getLocalJacobian();
        Jacobian dJ = _joint->getLocalJacobianTimeDeriv();

        //--------------------------------------------------------------------------
        // Test T
        //--------------------------------------------------------------------------
        EXPECT_TRUE(math::verifyTransform(T));

        //--------------------------------------------------------------------------
        // Test analytic Jacobian and numerical Jacobian
        // J == numericalJ
        //--------------------------------------------------------------------------
        Jacobian numericJ = Jacobian::Zero(6,dof);
        for (int i = 0; i < dof; ++i)
        {
            // a
            Eigen::VectorXd q_a = q;
            _joint->set_q(q_a);
            bodyNode.updateTransform();
            Eigen::Isometry3d T_a = _joint->getLocalTransform();

            // b
            Eigen::VectorXd q_b = q;
            q_b(i) += dt;
            _joint->set_q(q_b);
            bodyNode.updateTransform();
            Eigen::Isometry3d T_b = _joint->getLocalTransform();

            //
            Eigen::Isometry3d Tinv_a = T_a.inverse();
            Eigen::Matrix4d Tinv_a_eigen = Tinv_a.matrix();

            // dTdq
            Eigen::Matrix4d T_a_eigen = T_a.matrix();
            Eigen::Matrix4d T_b_eigen = T_b.matrix();
            Eigen::Matrix4d dTdq_eigen = (T_b_eigen - T_a_eigen) / dt;
            //Matrix4d dTdq_eigen = (T_b_eigen * T_a_eigen.inverse()) / dt;

            // J(i)
            Eigen::Matrix4d Ji_4x4matrix_eigen = Tinv_a_eigen * dTdq_eigen;
            Eigen::Vector6d Ji;
            Ji[0] = Ji_4x4matrix_eigen(2,1);
            Ji[1] = Ji_4x4matrix_eigen(0,2);
            Ji[2] = Ji_4x4matrix_eigen(1,0);
            Ji[3] = Ji_4x4matrix_eigen(0,3);
            Ji[4] = Ji_4x4matrix_eigen(1,3);
            Ji[5] = Ji_4x4matrix_eigen(2,3);
            numericJ.col(i) = Ji;
        }

        for (int i = 0; i < dof; ++i)
            for (int j = 0; j < 6; ++j)
                EXPECT_NEAR(J.col(i)(j), numericJ.col(i)(j), JOINT_TOL);

        //--------------------------------------------------------------------------
        // Test first time derivative of analytic Jacobian and numerical Jacobian
        // dJ == numerical_dJ
        //--------------------------------------------------------------------------
        Jacobian numeric_dJ = Jacobian::Zero(6,dof);
        for (int i = 0; i < dof; ++i)
        {

            // a
            Eigen::VectorXd q_a = q;
            _joint->set_q(q_a);
            bodyNode.updateVelocity();
            Jacobian J_a = _joint->getLocalJacobian();

            // b
            Eigen::VectorXd q_b = q;
            q_b(i) += dt;
            _joint->set_q(q_b);
            bodyNode.updateVelocity();
            Jacobian J_b = _joint->getLocalJacobian();

            //
            Jacobian dJ_dq = (J_b - J_a) / dt;

            // J(i)
            numeric_dJ += dJ_dq * dq(i);
        }

        for (int i = 0; i < dof; ++i)
            for (int j = 0; j < 6; ++j)
                EXPECT_NEAR(dJ.col(i)(j), numeric_dJ.col(i)(j), JOINT_TOL);
    }
}

// 0-dof joint
TEST_F(JOINTS, WELD_JOINT)
{
    WeldJoint weldJoint;

    kinematicsTest(&weldJoint);
}

// 1-dof joint
TEST_F(JOINTS, REVOLUTE_JOINT)
{
    RevoluteJoint revJoint;

    kinematicsTest(&revJoint);
}

// 1-dof joint
TEST_F(JOINTS, PRISMATIC_JOINT)
{
    PrismaticJoint priJoint;

    kinematicsTest(&priJoint);
}

// 1-dof joint
TEST_F(JOINTS, SCREW_JOINT)
{
    ScrewJoint screwJoint;

    kinematicsTest(&screwJoint);
}

// 2-dof joint
TEST_F(JOINTS, UNIVERSAL_JOINT)
{
    UniversalJoint univJoint;

    kinematicsTest(&univJoint);
}

// 3-dof joint
TEST_F(JOINTS, BALL_JOINT)
{
    BallJoint ballJoint;

    kinematicsTest(&ballJoint);
}

// 3-dof joint
TEST_F(JOINTS, EULER_JOINT)
{
    EulerJoint eulerJoint;

    eulerJoint.setAxisOrder(EulerJoint::AO_XYZ);
    kinematicsTest(&eulerJoint);

    eulerJoint.setAxisOrder(EulerJoint::AO_ZYX);
    kinematicsTest(&eulerJoint);
}

// 3-dof joint
TEST_F(JOINTS, TRANSLATIONAL_JOINT)
{
    TranslationalJoint translationalJoint;

    kinematicsTest(&translationalJoint);
}

// 6-dof joint
TEST_F(JOINTS, FREE_JOINT)
{
    FreeJoint freeJoint;

    kinematicsTest(&freeJoint);
}

TEST_F(JOINTS, POSITION_LIMIT)
{
    double tol = 1e-4;

    simulation::World* myWorld = utils::SkelParser::readSkelFile(
            DART_DATA_PATH"/skel/test/joint_limit_test.skel");
    EXPECT_TRUE(myWorld != NULL);

    myWorld->setGravity(Eigen::Vector3d(0.0, 0.0, -9.81));

    dynamics::Skeleton* pendulum = myWorld->getSkeleton("double_pendulum");
    EXPECT_TRUE(pendulum != NULL);

    dynamics::Joint* joint0 = pendulum->getJoint("joint0");
    dynamics::Joint* joint1 = pendulum->getJoint("joint1");

    EXPECT_TRUE(joint0 != NULL);
    EXPECT_TRUE(joint1 != NULL);

    double limit0 = DART_PI / 6.0;
    double limit1 = DART_PI / 6.0;

    joint0->setPositionLimited(true);
    joint0->getGenCoord(0)->set_qMin(-limit0);
    joint0->getGenCoord(0)->set_qMax(limit0);

    joint1->setPositionLimited(true);
    joint1->getGenCoord(0)->set_qMin(-limit1);
    joint1->getGenCoord(0)->set_qMax(limit1);

    double simTime = 2.0;
    double timeStep = myWorld->getTimeStep();
    int nSteps = simTime / timeStep;

    for (int i = 0; i < nSteps; i++)
    {
        myWorld->step();
    }

    double jointPos0 = joint0->getGenCoord(0)->get_q();
    double jointPos1 = joint1->getGenCoord(0)->get_q();

    double jointVel0 = joint0->getGenCoord(0)->get_dq();
    double jointVel1 = joint1->getGenCoord(0)->get_dq();

    // NOTE: The ideal result is that the joint position limit was obeyed with
    //       zero tolerance. To do so, DART should correct the joint limit
    //       violation in which is not implemented yet. This feature should be
    //       added in DART.
    EXPECT_NEAR(jointPos0, -limit0, 1e-4);
    EXPECT_NEAR(jointPos1, -limit1, 1e-3);

    EXPECT_NEAR(jointVel0, 0.0, tol);
    EXPECT_NEAR(jointVel1, 0.0, tol);
}

/******************************************************************************/
int main(int argc, char* argv[])
{
	::testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}

<commit_msg>Fix memory error caused by last commit<commit_after>/*
 * Copyright (c) 2011, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s): Jeongseok Lee <jslee02@gmail.com>
 * Date: 05/23/2013
 *
 * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab
 *
 * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
 * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   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 HOLDER 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.
 */

#include <iostream>
#include <gtest/gtest.h>
#include "TestHelpers.h"

#include "math/Geometry.h"
#include "math/Helpers.h"
#include "dynamics/BallJoint.h"
#include "dynamics/FreeJoint.h"
#include "dynamics/PrismaticJoint.h"
#include "dynamics/RevoluteJoint.h"
#include "dynamics/Skeleton.h"
#include "dynamics/TranslationalJoint.h"
#include "dynamics/UniversalJoint.h"
#include "dynamics/WeldJoint.h"
#include "dynamics/EulerJoint.h"
#include "dynamics/ScrewJoint.h"
#include "dynamics/BodyNode.h"
#include "dynamics/Skeleton.h"
#include "simulation/World.h"
#include "utils/Paths.h"
#include "utils/SkelParser.h"

using namespace dart;
using namespace math;
using namespace dynamics;

#define JOINT_TOL 0.01

/******************************************************************************/
class JOINTS : public testing::Test
{
public:
    void kinematicsTest(Joint* _joint);
};

/******************************************************************************/
void JOINTS::kinematicsTest(Joint* _joint)
{
    BodyNode bodyNode;
    bodyNode.setParentJoint(_joint);

    Skeleton skeleton;
    skeleton.addBodyNode(&bodyNode);
    skeleton.init();

    int dof = _joint->getNumGenCoords();

    //--------------------------------------------------------------------------
    //
    //--------------------------------------------------------------------------
    VectorXd q = VectorXd::Zero(dof);
    VectorXd dq = VectorXd::Zero(dof);
    VectorXd ddq = VectorXd::Zero(dof);

    for (int idxTest = 0; idxTest < 100; ++idxTest)
    {
        double dt = 0.000001;

        for (int i = 0; i < dof; ++i)
        {
            q(i) = random(-DART_PI, DART_PI);
            dq(i) = random(-DART_PI, DART_PI);
            ddq(i) = random(-DART_PI, DART_PI);
        }

        _joint->set_q(q);
        _joint->set_dq(dq);
        _joint->set_ddq(ddq);

        bodyNode.updateTransform();
        bodyNode.updateVelocity();
        bodyNode.updateEta();
        bodyNode.updateAcceleration();

        if (_joint->getNumGenCoords() == 0)
            return;

        Eigen::Isometry3d T = _joint->getLocalTransform();
        Jacobian J = _joint->getLocalJacobian();
        Jacobian dJ = _joint->getLocalJacobianTimeDeriv();

        //--------------------------------------------------------------------------
        // Test T
        //--------------------------------------------------------------------------
        EXPECT_TRUE(math::verifyTransform(T));

        //--------------------------------------------------------------------------
        // Test analytic Jacobian and numerical Jacobian
        // J == numericalJ
        //--------------------------------------------------------------------------
        Jacobian numericJ = Jacobian::Zero(6,dof);
        for (int i = 0; i < dof; ++i)
        {
            // a
            Eigen::VectorXd q_a = q;
            _joint->set_q(q_a);
            bodyNode.updateTransform();
            Eigen::Isometry3d T_a = _joint->getLocalTransform();

            // b
            Eigen::VectorXd q_b = q;
            q_b(i) += dt;
            _joint->set_q(q_b);
            bodyNode.updateTransform();
            Eigen::Isometry3d T_b = _joint->getLocalTransform();

            //
            Eigen::Isometry3d Tinv_a = T_a.inverse();
            Eigen::Matrix4d Tinv_a_eigen = Tinv_a.matrix();

            // dTdq
            Eigen::Matrix4d T_a_eigen = T_a.matrix();
            Eigen::Matrix4d T_b_eigen = T_b.matrix();
            Eigen::Matrix4d dTdq_eigen = (T_b_eigen - T_a_eigen) / dt;
            //Matrix4d dTdq_eigen = (T_b_eigen * T_a_eigen.inverse()) / dt;

            // J(i)
            Eigen::Matrix4d Ji_4x4matrix_eigen = Tinv_a_eigen * dTdq_eigen;
            Eigen::Vector6d Ji;
            Ji[0] = Ji_4x4matrix_eigen(2,1);
            Ji[1] = Ji_4x4matrix_eigen(0,2);
            Ji[2] = Ji_4x4matrix_eigen(1,0);
            Ji[3] = Ji_4x4matrix_eigen(0,3);
            Ji[4] = Ji_4x4matrix_eigen(1,3);
            Ji[5] = Ji_4x4matrix_eigen(2,3);
            numericJ.col(i) = Ji;
        }

        for (int i = 0; i < dof; ++i)
            for (int j = 0; j < 6; ++j)
                EXPECT_NEAR(J.col(i)(j), numericJ.col(i)(j), JOINT_TOL);

        //--------------------------------------------------------------------------
        // Test first time derivative of analytic Jacobian and numerical Jacobian
        // dJ == numerical_dJ
        //--------------------------------------------------------------------------
        Jacobian numeric_dJ = Jacobian::Zero(6,dof);
        for (int i = 0; i < dof; ++i)
        {

            // a
            Eigen::VectorXd q_a = q;
            _joint->set_q(q_a);
            bodyNode.updateVelocity();
            Jacobian J_a = _joint->getLocalJacobian();

            // b
            Eigen::VectorXd q_b = q;
            q_b(i) += dt;
            _joint->set_q(q_b);
            bodyNode.updateVelocity();
            Jacobian J_b = _joint->getLocalJacobian();

            //
            Jacobian dJ_dq = (J_b - J_a) / dt;

            // J(i)
            numeric_dJ += dJ_dq * dq(i);
        }

        for (int i = 0; i < dof; ++i)
            for (int j = 0; j < 6; ++j)
                EXPECT_NEAR(dJ.col(i)(j), numeric_dJ.col(i)(j), JOINT_TOL);
    }
}

// 0-dof joint
TEST_F(JOINTS, WELD_JOINT)
{
    WeldJoint* weldJoint = new WeldJoint;

    kinematicsTest(weldJoint);
}

// 1-dof joint
TEST_F(JOINTS, REVOLUTE_JOINT)
{
    RevoluteJoint* revJoint = new RevoluteJoint;

    kinematicsTest(revJoint);
}

// 1-dof joint
TEST_F(JOINTS, PRISMATIC_JOINT)
{
    PrismaticJoint* priJoint = new PrismaticJoint;

    kinematicsTest(priJoint);
}

// 1-dof joint
TEST_F(JOINTS, SCREW_JOINT)
{
    ScrewJoint* screwJoint = new ScrewJoint;

    kinematicsTest(screwJoint);
}

// 2-dof joint
TEST_F(JOINTS, UNIVERSAL_JOINT)
{
    UniversalJoint* univJoint = new UniversalJoint;

    kinematicsTest(univJoint);
}

// 3-dof joint
TEST_F(JOINTS, BALL_JOINT)
{
    BallJoint* ballJoint = new BallJoint;

    kinematicsTest(ballJoint);
}

// 3-dof joint
TEST_F(JOINTS, EULER_JOINT)
{
    EulerJoint* eulerJoint1 = new EulerJoint;

    eulerJoint1->setAxisOrder(EulerJoint::AO_XYZ);
    kinematicsTest(eulerJoint1);

    EulerJoint* eulerJoint2 = new EulerJoint;

    eulerJoint2->setAxisOrder(EulerJoint::AO_ZYX);
    kinematicsTest(eulerJoint2);
}

// 3-dof joint
TEST_F(JOINTS, TRANSLATIONAL_JOINT)
{
    TranslationalJoint* translationalJoint = new TranslationalJoint;

    kinematicsTest(translationalJoint);
}

// 6-dof joint
TEST_F(JOINTS, FREE_JOINT)
{
    FreeJoint* freeJoint = new FreeJoint;

    kinematicsTest(freeJoint);
}

TEST_F(JOINTS, POSITION_LIMIT)
{
    double tol = 1e-4;

    simulation::World* myWorld = utils::SkelParser::readSkelFile(
            DART_DATA_PATH"/skel/test/joint_limit_test.skel");
    EXPECT_TRUE(myWorld != NULL);

    myWorld->setGravity(Eigen::Vector3d(0.0, 0.0, -9.81));

    dynamics::Skeleton* pendulum = myWorld->getSkeleton("double_pendulum");
    EXPECT_TRUE(pendulum != NULL);

    dynamics::Joint* joint0 = pendulum->getJoint("joint0");
    dynamics::Joint* joint1 = pendulum->getJoint("joint1");

    EXPECT_TRUE(joint0 != NULL);
    EXPECT_TRUE(joint1 != NULL);

    double limit0 = DART_PI / 6.0;
    double limit1 = DART_PI / 6.0;

    joint0->setPositionLimited(true);
    joint0->getGenCoord(0)->set_qMin(-limit0);
    joint0->getGenCoord(0)->set_qMax(limit0);

    joint1->setPositionLimited(true);
    joint1->getGenCoord(0)->set_qMin(-limit1);
    joint1->getGenCoord(0)->set_qMax(limit1);

    double simTime = 2.0;
    double timeStep = myWorld->getTimeStep();
    int nSteps = simTime / timeStep;

    for (int i = 0; i < nSteps; i++)
    {
        myWorld->step();
    }

    double jointPos0 = joint0->getGenCoord(0)->get_q();
    double jointPos1 = joint1->getGenCoord(0)->get_q();

    double jointVel0 = joint0->getGenCoord(0)->get_dq();
    double jointVel1 = joint1->getGenCoord(0)->get_dq();

    // NOTE: The ideal result is that the joint position limit was obeyed with
    //       zero tolerance. To do so, DART should correct the joint limit
    //       violation in which is not implemented yet. This feature should be
    //       added in DART.
    EXPECT_NEAR(jointPos0, -limit0, 1e-4);
    EXPECT_NEAR(jointPos1, -limit1, 1e-3);

    EXPECT_NEAR(jointVel0, 0.0, tol);
    EXPECT_NEAR(jointVel1, 0.0, tol);
}

/******************************************************************************/
int main(int argc, char* argv[])
{
	::testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}

<|endoftext|>


<commit_before>/*
 * Distributed under the OSI-approved Apache License, Version 2.0.  See
 * accompanying file Copyright.txt for details.
 */
#include <cstdint>
#include <cstring>
#include <ctime>

#include <chrono>
#include <iostream>
#include <stdexcept>
#include <thread>

#include <adios2.h>

#include <gtest/gtest.h>

#include "TestData.h"

#include "ParseArgs.h"

class CommonWriteTest : public ::testing::Test
{
public:
    CommonWriteTest() = default;
};

#if ADIOS2_USE_MPI
MPI_Comm testComm;
#endif

// ADIOS2 COMMON write
TEST_F(CommonWriteTest, ADIOS2CommonWrite)
{
    // form a mpiSize * Nx 1D array
    int mpiRank = 0, mpiSize = 1;

#if ADIOS2_USE_MPI
    MPI_Comm_rank(testComm, &mpiRank);
    MPI_Comm_size(testComm, &mpiSize);
#endif

    // Write test data using ADIOS2

#if ADIOS2_USE_MPI
    adios2::ADIOS adios(testComm);
#else
    adios2::ADIOS adios;
#endif
    adios2::IO io = adios.DeclareIO("TestIO");

    // Declare 1D variables (NumOfProcesses * Nx)
    // The local process' part (start, count) can be defined now or later
    // before Write().

    /* we'll be dropping the size for some vars */
    if (VaryingDataSize)
    {
        assert(Nx > NSteps + mpiSize);
    }

    {
        adios2::Dims shape{static_cast<unsigned int>(Nx * mpiSize)};
        adios2::Dims start{static_cast<unsigned int>(Nx * mpiRank)};
        adios2::Dims count{static_cast<unsigned int>(Nx)};
        adios2::Dims shape2{static_cast<unsigned int>(Nx * mpiSize), 2};
        adios2::Dims start2{static_cast<unsigned int>(Nx * mpiRank), 0};
        adios2::Dims count2{static_cast<unsigned int>(Nx), 2};
        adios2::Dims shape3{2, static_cast<unsigned int>(Nx * mpiSize)};
        adios2::Dims start3{0, static_cast<unsigned int>(Nx * mpiRank)};
        adios2::Dims count3{2, static_cast<unsigned int>(Nx)};
        adios2::Dims time_shape{static_cast<unsigned int>(mpiSize)};
        adios2::Dims time_start{static_cast<unsigned int>(mpiRank)};
        adios2::Dims time_count{1};

        (void)io.DefineVariable<double>("scalar_r64");
        (void)io.DefineVariable<int8_t>("i8", {}, {}, count);
        (void)io.DefineVariable<int16_t>("i16", {}, {}, count);
        (void)io.DefineVariable<int32_t>("i32", {}, {}, count);
        (void)io.DefineVariable<int64_t>("i64", {}, {}, count);
        auto local_var_r32 = io.DefineVariable<float>("r32", {}, {}, count);
        auto local_var_r64 = io.DefineVariable<double>("r64", {}, {}, count);
        (void)io.DefineVariable<std::complex<float>>("c32", {}, {}, count);
        (void)io.DefineVariable<std::complex<double>>("c64", {}, {}, count);
        auto local_var_r64_2d =
            io.DefineVariable<double>("r64_2d", {}, {}, count2);
        auto local_var_r64_2d_rev =
            io.DefineVariable<double>("r64_2d_rev", {}, {}, count3);
        (void)io.DefineVariable<int64_t>("time", time_shape, time_start,
                                         time_count);
        if (CompressZfp)
        {
            adios2::Operator ZfpOp =
                adios.DefineOperator("zfpCompressor", "zfp");
            local_var_r32.AddOperation(ZfpOp, {{"rate", "20"}});
            local_var_r64.AddOperation(ZfpOp, {{"rate", "20"}});
            local_var_r64_2d.AddOperation(ZfpOp, {{"rate", "20"}});
            local_var_r64_2d_rev.AddOperation(ZfpOp, {{"rate", "20"}});
        }
    }

    // Create the Engine
    io.SetEngine(engine);
    io.SetParameters(engineParams);

    adios2::Engine engine = io.Open(fname, adios2::Mode::Write);

    for (size_t step = 0; step < NSteps; ++step)
    {
        // Generate test data for each process uniquely
        generateCommonTestData((int)step, mpiRank, mpiSize, (int)Nx, (int)Nx);

        engine.BeginStep();
        // Retrieve the variables that previously went out of scope
        auto scalar_r64 = io.InquireVariable<double>("scalar_r64");
        auto var_i8 = io.InquireVariable<int8_t>("i8");
        auto var_i16 = io.InquireVariable<int16_t>("i16");
        auto var_i32 = io.InquireVariable<int32_t>("i32");
        auto var_i64 = io.InquireVariable<int64_t>("i64");
        auto var_r32 = io.InquireVariable<float>("r32");
        auto var_r64 = io.InquireVariable<double>("r64");
        auto var_c32 = io.InquireVariable<std::complex<float>>("c32");
        auto var_c64 = io.InquireVariable<std::complex<double>>("c64");
        auto var_r64_2d = io.InquireVariable<double>("r64_2d");
        auto var_r64_2d_rev = io.InquireVariable<double>("r64_2d_rev");
        auto var_time = io.InquireVariable<int64_t>("time");

        // // Make a 1D selection to describe the local dimensions of the
        // // variable we write
        adios2::Box<adios2::Dims> sel_shrinking({}, {Nx - step - mpiRank});
        // adios2::Box<adios2::Dims> sel({mpiRank * Nx}, {Nx});
        // adios2::Box<adios2::Dims> sel2({mpiRank * Nx, 0}, {Nx, 2});
        // adios2::Box<adios2::Dims> sel3({0, mpiRank * Nx}, {2, Nx});
        // adios2::Box<adios2::Dims> sel_time(
        //     {static_cast<unsigned long>(mpiRank)}, {1});
        if (VaryingDataSize)
        {
            var_i8.SetSelection(sel_shrinking);
        }
        // var_i16.SetSelection(sel);
        // var_i32.SetSelection(sel);
        // var_i64.SetSelection(sel);
        // var_r32.SetSelection(sel);
        // var_r64.SetSelection(sel);
        // var_c32.SetSelection(sel);
        // var_c64.SetSelection(sel);
        // var_r64_2d.SetSelection(sel2);
        // var_r64_2d_rev.SetSelection(sel3);
        // var_time.SetSelection(sel_time);

        // Write each one
        // fill in the variable with values from starting index to
        // starting index + count
        const adios2::Mode sync = adios2::Mode::Deferred;

        if (mpiRank == 0)
            engine.Put(scalar_r64, data_scalar_R64);
        engine.Put(var_i8, data_I8.data(), sync);
        engine.Put(var_i16, data_I16.data(), sync);
        engine.Put(var_i32, data_I32.data(), sync);
        engine.Put(var_i64, data_I64.data(), sync);
        for (int index = 0; index < LocalCount; index++)
        {
            engine.Put(var_r32, data_R32.data(), sync);
            for (int i = 0; i < data_R32.size(); i++)
            {
                data_R32[i] += 1000.0;
            }
        }
        engine.Put(var_r64, data_R64.data(), sync);
        engine.Put(var_c32, data_C32.data(), sync);
        engine.Put(var_c64, data_C64.data(), sync);
        engine.Put(var_r64_2d, &data_R64_2d[0], sync);
        engine.Put(var_r64_2d_rev, &data_R64_2d_rev[0], sync);
        // Advance to the next time step
        std::time_t localtime = std::time(NULL);
        engine.Put(var_time, (int64_t *)&localtime);
        engine.EndStep();
        std::this_thread::sleep_for(std::chrono::milliseconds(
            DelayMS)); /* sleep for DelayMS milliseconds */
    }

    // Close the file
    engine.Close();
}

int main(int argc, char **argv)
{
#if ADIOS2_USE_MPI
    MPI_Init(nullptr, nullptr);

    int key;
    MPI_Comm_rank(MPI_COMM_WORLD, &key);

    const unsigned int color = 1;
    MPI_Comm_split(MPI_COMM_WORLD, color, key, &testComm);
#endif

    int result;
    ::testing::InitGoogleTest(&argc, argv);

    DelayMS = 0; // zero default for common writer

    ParseArgs(argc, argv);

    result = RUN_ALL_TESTS();

#if ADIOS2_USE_MPI
    MPI_Finalize();
#endif

    return result;
}
<commit_msg>Work around a bug in BP marshalling in SST<commit_after>/*
 * Distributed under the OSI-approved Apache License, Version 2.0.  See
 * accompanying file Copyright.txt for details.
 */
#include <cstdint>
#include <cstring>
#include <ctime>

#include <chrono>
#include <iostream>
#include <stdexcept>
#include <thread>

#include <adios2.h>

#include <gtest/gtest.h>

#include "TestData.h"

#include "ParseArgs.h"

class CommonWriteTest : public ::testing::Test
{
public:
    CommonWriteTest() = default;
};

#if ADIOS2_USE_MPI
MPI_Comm testComm;
#endif

// ADIOS2 COMMON write
TEST_F(CommonWriteTest, ADIOS2CommonWrite)
{
    // form a mpiSize * Nx 1D array
    int mpiRank = 0, mpiSize = 1;

#if ADIOS2_USE_MPI
    MPI_Comm_rank(testComm, &mpiRank);
    MPI_Comm_size(testComm, &mpiSize);
#endif

    // Write test data using ADIOS2

#if ADIOS2_USE_MPI
    adios2::ADIOS adios(testComm);
#else
    adios2::ADIOS adios;
#endif
    adios2::IO io = adios.DeclareIO("TestIO");

    // Declare 1D variables (NumOfProcesses * Nx)
    // The local process' part (start, count) can be defined now or later
    // before Write().

    /* we'll be dropping the size for some vars */
    if (VaryingDataSize)
    {
        assert(Nx > NSteps + mpiSize);
    }

    {
        adios2::Dims shape{static_cast<unsigned int>(Nx * mpiSize)};
        adios2::Dims start{static_cast<unsigned int>(Nx * mpiRank)};
        adios2::Dims count{static_cast<unsigned int>(Nx)};
        adios2::Dims shape2{static_cast<unsigned int>(Nx * mpiSize), 2};
        adios2::Dims start2{static_cast<unsigned int>(Nx * mpiRank), 0};
        adios2::Dims count2{static_cast<unsigned int>(Nx), 2};
        adios2::Dims shape3{2, static_cast<unsigned int>(Nx * mpiSize)};
        adios2::Dims start3{0, static_cast<unsigned int>(Nx * mpiRank)};
        adios2::Dims count3{2, static_cast<unsigned int>(Nx)};
        adios2::Dims time_shape{static_cast<unsigned int>(mpiSize)};
        adios2::Dims time_start{static_cast<unsigned int>(mpiRank)};
        adios2::Dims time_count{1};

        (void)io.DefineVariable<double>("scalar_r64");
        (void)io.DefineVariable<int8_t>("i8", {}, {}, count);
        (void)io.DefineVariable<int16_t>("i16", {}, {}, count);
        (void)io.DefineVariable<int32_t>("i32", {}, {}, count);
        (void)io.DefineVariable<int64_t>("i64", {}, {}, count);
        auto local_var_r32 = io.DefineVariable<float>("r32", {}, {}, count);
        auto local_var_r64 = io.DefineVariable<double>("r64", {}, {}, count);
        (void)io.DefineVariable<std::complex<float>>("c32", {}, {}, count);
        (void)io.DefineVariable<std::complex<double>>("c64", {}, {}, count);
        auto local_var_r64_2d =
            io.DefineVariable<double>("r64_2d", {}, {}, count2);
        auto local_var_r64_2d_rev =
            io.DefineVariable<double>("r64_2d_rev", {}, {}, count3);
        (void)io.DefineVariable<int64_t>("time", time_shape, time_start,
                                         time_count);
        if (CompressZfp)
        {
            adios2::Operator ZfpOp =
                adios.DefineOperator("zfpCompressor", "zfp");
            local_var_r32.AddOperation(ZfpOp, {{"rate", "20"}});
            local_var_r64.AddOperation(ZfpOp, {{"rate", "20"}});
            local_var_r64_2d.AddOperation(ZfpOp, {{"rate", "20"}});
            local_var_r64_2d_rev.AddOperation(ZfpOp, {{"rate", "20"}});
        }
    }

    // Create the Engine
    io.SetEngine(engine);
    io.SetParameters(engineParams);

    adios2::Engine engine = io.Open(fname, adios2::Mode::Write);

    for (size_t step = 0; step < NSteps; ++step)
    {
        // Generate test data for each process uniquely
        generateCommonTestData((int)step, mpiRank, mpiSize, (int)Nx, (int)Nx);

        engine.BeginStep();
        // Retrieve the variables that previously went out of scope
        auto scalar_r64 = io.InquireVariable<double>("scalar_r64");
        auto var_i8 = io.InquireVariable<int8_t>("i8");
        auto var_i16 = io.InquireVariable<int16_t>("i16");
        auto var_i32 = io.InquireVariable<int32_t>("i32");
        auto var_i64 = io.InquireVariable<int64_t>("i64");
        auto var_r32 = io.InquireVariable<float>("r32");
        auto var_r64 = io.InquireVariable<double>("r64");
        auto var_c32 = io.InquireVariable<std::complex<float>>("c32");
        auto var_c64 = io.InquireVariable<std::complex<double>>("c64");
        auto var_r64_2d = io.InquireVariable<double>("r64_2d");
        auto var_r64_2d_rev = io.InquireVariable<double>("r64_2d_rev");
        auto var_time = io.InquireVariable<int64_t>("time");

        // // Make a 1D selection to describe the local dimensions of the
        // // variable we write
        adios2::Box<adios2::Dims> sel_shrinking({}, {Nx - step - mpiRank});
        // adios2::Box<adios2::Dims> sel({mpiRank * Nx}, {Nx});
        // adios2::Box<adios2::Dims> sel2({mpiRank * Nx, 0}, {Nx, 2});
        // adios2::Box<adios2::Dims> sel3({0, mpiRank * Nx}, {2, Nx});
        // adios2::Box<adios2::Dims> sel_time(
        //     {static_cast<unsigned long>(mpiRank)}, {1});
        if (VaryingDataSize)
        {
            var_i8.SetSelection(sel_shrinking);
        }
        // var_i16.SetSelection(sel);
        // var_i32.SetSelection(sel);
        // var_i64.SetSelection(sel);
        // var_r32.SetSelection(sel);
        // var_r64.SetSelection(sel);
        // var_c32.SetSelection(sel);
        // var_c64.SetSelection(sel);
        // var_r64_2d.SetSelection(sel2);
        // var_r64_2d_rev.SetSelection(sel3);
        // var_time.SetSelection(sel_time);

        // Write each one
        // fill in the variable with values from starting index to
        // starting index + count
        const adios2::Mode sync = adios2::Mode::Deferred;

        if (mpiRank == 0)
            engine.Put(scalar_r64, data_scalar_R64);
        engine.Put(var_i8, data_I8.data(), sync);
        engine.Put(var_i16, data_I16.data(), sync);
        engine.Put(var_i32, data_I32.data(), sync);
        engine.Put(var_i64, data_I64.data(), sync);
        engine.Put(var_r32, data_R32.data(), sync);
        for (int index = 1; index < LocalCount; index++)
        {
            for (int i = 0; i < data_R32.size(); i++)
            {
                data_R32[i] += 1000.0;
            }
            engine.Put(var_r32, data_R32.data(), sync);
        }
        engine.Put(var_r64, data_R64.data(), sync);
        engine.Put(var_c32, data_C32.data(), sync);
        engine.Put(var_c64, data_C64.data(), sync);
        engine.Put(var_r64_2d, &data_R64_2d[0], sync);
        engine.Put(var_r64_2d_rev, &data_R64_2d_rev[0], sync);
        // Advance to the next time step
        std::time_t localtime = std::time(NULL);
        engine.Put(var_time, (int64_t *)&localtime);
        engine.EndStep();
        std::this_thread::sleep_for(std::chrono::milliseconds(
            DelayMS)); /* sleep for DelayMS milliseconds */
    }

    // Close the file
    engine.Close();
}

int main(int argc, char **argv)
{
#if ADIOS2_USE_MPI
    MPI_Init(nullptr, nullptr);

    int key;
    MPI_Comm_rank(MPI_COMM_WORLD, &key);

    const unsigned int color = 1;
    MPI_Comm_split(MPI_COMM_WORLD, color, key, &testComm);
#endif

    int result;
    ::testing::InitGoogleTest(&argc, argv);

    DelayMS = 0; // zero default for common writer

    ParseArgs(argc, argv);

    result = RUN_ALL_TESTS();

#if ADIOS2_USE_MPI
    MPI_Finalize();
#endif

    return result;
}
<|endoftext|>


<commit_before>#define PTR_MAP_ENABLE_MULTI_MAP 1

typedef u32 MapIndex;

struct MapFindResult {
	MapIndex hash_index;
	MapIndex entry_prev;
	MapIndex entry_index;
};

enum : MapIndex { MAP_SENTINEL = ~(MapIndex)0 };

template <typename K, typename V>
struct PtrMapEntry {
	static_assert(sizeof(K) == sizeof(void *), "Key size must be pointer size");

	K        key;
	V        value;
	MapIndex next;
};

template <typename K, typename V>
struct PtrMap {
	Slice<MapIndex>           hashes;
	Array<PtrMapEntry<K, V> > entries;
};

u32 ptr_map_hash_key(uintptr key) {
#if defined(GB_ARCH_64_BIT)
	u64 x = (u64)key;
	u8 count = (u8)(x >> 59);
	x ^= x >> (5 + count);
	x *= 12605985483714917081ull;
	return (u32)(x ^ (x >> 43));
#elif defined(GB_ARCH_32_BIT)
	u32 state = ((u32)key) * 747796405u + 2891336453u;
	u32 word = ((state >> ((state >> 28u) + 4u)) ^ state) * 277803737u;
	return (word >> 22u) ^ word;
#endif
}
u32 ptr_map_hash_key(void const *key) {
	return ptr_map_hash_key((uintptr)key);
}

template <typename K, typename V> void map_init             (PtrMap<K, V> *h, gbAllocator a, isize capacity = 16);
template <typename K, typename V> void map_destroy          (PtrMap<K, V> *h);
template <typename K, typename V> V *  map_get              (PtrMap<K, V> *h, K key);
template <typename K, typename V> void map_set              (PtrMap<K, V> *h, K key, V const &value);
template <typename K, typename V> void map_remove           (PtrMap<K, V> *h, K key);
template <typename K, typename V> void map_clear            (PtrMap<K, V> *h);
template <typename K, typename V> void map_grow             (PtrMap<K, V> *h);
template <typename K, typename V> void map_rehash           (PtrMap<K, V> *h, isize new_count);
template <typename K, typename V> void map_reserve          (PtrMap<K, V> *h, isize cap);

#if PTR_MAP_ENABLE_MULTI_MAP
// Mutlivalued map procedure
template <typename K, typename V> PtrMapEntry<K, V> * multi_map_find_first(PtrMap<K, V> *h, K key);
template <typename K, typename V> PtrMapEntry<K, V> * multi_map_find_next (PtrMap<K, V> *h, PtrMapEntry<K, V> *e);

template <typename K, typename V> isize multi_map_count     (PtrMap<K, V> *h, K key);
template <typename K, typename V> void  multi_map_get_all   (PtrMap<K, V> *h, K key, V *items);
template <typename K, typename V> void  multi_map_insert    (PtrMap<K, V> *h, K key, V const &value);
template <typename K, typename V> void  multi_map_remove    (PtrMap<K, V> *h, K key, PtrMapEntry<K, V> *e);
template <typename K, typename V> void  multi_map_remove_all(PtrMap<K, V> *h, K key);
#endif

template <typename K, typename V>
gb_inline void map_init(PtrMap<K, V> *h, gbAllocator a, isize capacity) {
	capacity = next_pow2_isize(capacity);
	slice_init(&h->hashes,  a, capacity);
	array_init(&h->entries, a, 0, capacity);
	for (isize i = 0; i < capacity; i++) {
		h->hashes.data[i] = MAP_SENTINEL;
	}
}

template <typename K, typename V>
gb_inline void map_destroy(PtrMap<K, V> *h) {
	slice_free(&h->hashes, h->entries.allocator);
	array_free(&h->entries);
}

template <typename K, typename V>
gb_internal MapIndex map__add_entry(PtrMap<K, V> *h, K key) {
	PtrMapEntry<K, V> e = {};
	e.key = key;
	e.next = MAP_SENTINEL;
	array_add(&h->entries, e);
	return cast(MapIndex)(h->entries.count-1);
}

template <typename K, typename V>
gb_internal MapFindResult map__find(PtrMap<K, V> *h, K key) {
	MapFindResult fr = {MAP_SENTINEL, MAP_SENTINEL, MAP_SENTINEL};
	if (h->hashes.count == 0) {
		return fr;
	}
	u32 hash = ptr_map_hash_key(key);
	fr.hash_index = cast(MapIndex)(hash & (h->hashes.count-1));
	fr.entry_index = h->hashes.data[fr.hash_index];
	while (fr.entry_index != MAP_SENTINEL) {
		if (h->entries.data[fr.entry_index].key == key) {
			return fr;
		}
		fr.entry_prev = fr.entry_index;
		fr.entry_index = h->entries.data[fr.entry_index].next;
	}
	return fr;
}

template <typename K, typename V>
gb_internal MapFindResult map__find_from_entry(PtrMap<K, V> *h, PtrMapEntry<K, V> *e) {
	MapFindResult fr = {MAP_SENTINEL, MAP_SENTINEL, MAP_SENTINEL};
	if (h->hashes.count == 0) {
		return fr;
	}
	u32 hash = ptr_map_hash_key(e->key);
	fr.hash_index  = cast(MapIndex)(hash & (h->hashes.count-1));
	fr.entry_index = h->hashes.data[fr.hash_index];
	while (fr.entry_index != MAP_SENTINEL) {
		if (&h->entries.data[fr.entry_index] == e) {
			return fr;
		}
		fr.entry_prev = fr.entry_index;
		fr.entry_index = h->entries.data[fr.entry_index].next;
	}
	return fr;
}

template <typename K, typename V>
gb_internal b32 map__full(PtrMap<K, V> *h) {
	return 0.75f * h->hashes.count <= h->entries.count;
}

template <typename K, typename V>
gb_inline void map_grow(PtrMap<K, V> *h) {
	isize new_count = gb_max(h->hashes.count<<1, 16);
	map_rehash(h, new_count);
}

template <typename K, typename V>
void map_reset_entries(PtrMap<K, V> *h) {
	for (isize i = 0; i < h->hashes.count; i++) {
		h->hashes.data[i] = MAP_SENTINEL;
	}
	for (isize i = 0; i < h->entries.count; i++) {
		MapFindResult fr;
		PtrMapEntry<K, V> *e = &h->entries.data[i];
		e->next = MAP_SENTINEL;
		fr = map__find_from_entry(h, e);
		if (fr.entry_prev == MAP_SENTINEL) {
			h->hashes[fr.hash_index] = cast(MapIndex)i;
		} else {
			h->entries[fr.entry_prev].next = cast(MapIndex)i;
		}
	}
}

template <typename K, typename V>
void map_reserve(PtrMap<K, V> *h, isize cap) {
	array_reserve(&h->entries, cap);
	if (h->entries.count*2 < h->hashes.count) {
		return;
	}
	slice_resize(&h->hashes, h->entries.allocator, cap*2);
	map_reset_entries(h);
}

template <typename K, typename V>
void map_rehash(PtrMap<K, V> *h, isize new_count) {
	map_reserve(h, new_count);
}

template <typename K, typename V>
V *map_get(PtrMap<K, V> *h, K key) {
	MapIndex index = map__find(h, key).entry_index;
	if (index != MAP_SENTINEL) {
		return &h->entries.data[index].value;
	}
	return nullptr;
}

template <typename K, typename V>
V &map_must_get(PtrMap<K, V> *h, K key) {
	MapIndex index = map__find(h, key).entry_index;
	GB_ASSERT(index != MAP_SENTINEL);
	return h->entries.data[index].value;
}

template <typename K, typename V>
void map_set(PtrMap<K, V> *h, K key, V const &value) {
	MapIndex index;
	MapFindResult fr;
	if (h->hashes.count == 0) {
		map_grow(h);
	}
	fr = map__find(h, key);
	if (fr.entry_index != MAP_SENTINEL) {
		index = fr.entry_index;
	} else {
		index = map__add_entry(h, key);
		if (fr.entry_prev != MAP_SENTINEL) {
			h->entries.data[fr.entry_prev].next = index;
		} else {
			h->hashes.data[fr.hash_index] = index;
		}
	}
	h->entries.data[index].value = value;

	if (map__full(h)) {
		map_grow(h);
	}
}

template <typename K, typename V>
void map__erase(PtrMap<K, V> *h, MapFindResult const &fr) {
	MapFindResult last;
	if (fr.entry_prev == MAP_SENTINEL) {
		h->hashes.data[fr.hash_index] = h->entries.data[fr.entry_index].next;
	} else {
		h->entries.data[fr.entry_prev].next = h->entries.data[fr.entry_index].next;
	}
	if (fr.entry_index == h->entries.count-1) {
		array_pop(&h->entries);
		return;
	}
	h->entries.data[fr.entry_index] = h->entries.data[h->entries.count-1];
	array_pop(&h->entries);

	last = map__find(h, h->entries.data[fr.entry_index].key);
	if (last.entry_prev != MAP_SENTINEL) {
		h->entries.data[last.entry_prev].next = fr.entry_index;
	} else {
		h->hashes.data[last.hash_index] = fr.entry_index;
	}
}

template <typename K, typename V>
void map_remove(PtrMap<K, V> *h, K key) {
	MapFindResult fr = map__find(h, key);
	if (fr.entry_index != MAP_SENTINEL) {
		map__erase(h, fr);
	}
}

template <typename K, typename V>
gb_inline void map_clear(PtrMap<K, V> *h) {
	array_clear(&h->entries);
	for (isize i = 0; i < h->hashes.count; i++) {
		h->hashes.data[i] = MAP_SENTINEL;
	}
}

#if PTR_MAP_ENABLE_MULTI_MAP
template <typename K, typename V>
PtrMapEntry<K, V> *multi_map_find_first(PtrMap<K, V> *h, K key) {
	MapIndex i = map__find(h, key).entry_index;
	if (i == MAP_SENTINEL) {
		return nullptr;
	}
	return &h->entries.data[i];
}

template <typename K, typename V>
PtrMapEntry<K, V> *multi_map_find_next(PtrMap<K, V> *h, PtrMapEntry<K, V> *e) {
	MapIndex i = e->next;
	while (i != MAP_SENTINEL) {
		if (h->entries.data[i].key == e->key) {
			return &h->entries.data[i];
		}
		i = h->entries.data[i].next;
	}
	return nullptr;
}

template <typename K, typename V>
isize multi_map_count(PtrMap<K, V> *h, K key) {
	isize count = 0;
	PtrMapEntry<K, V> *e = multi_map_find_first(h, key);
	while (e != nullptr) {
		count++;
		e = multi_map_find_next(h, e);
	}
	return count;
}

template <typename K, typename V>
void multi_map_get_all(PtrMap<K, V> *h, K key, V *items) {
	isize i = 0;
	PtrMapEntry<K, V> *e = multi_map_find_first(h, key);
	while (e != nullptr) {
		items[i++] = e->value;
		e = multi_map_find_next(h, e);
	}
}

template <typename K, typename V>
void multi_map_insert(PtrMap<K, V> *h, K key, V const &value) {
	MapFindResult fr;
	MapIndex i;
	if (h->hashes.count == 0) {
		map_grow(h);
	}
	// Make
	fr = map__find(h, key);
	i = map__add_entry(h, key);
	if (fr.entry_prev == MAP_SENTINEL) {
		h->hashes.data[fr.hash_index] = i;
	} else {
		h->entries.data[fr.entry_prev].next = i;
	}
	h->entries.data[i].next = fr.entry_index;
	h->entries.data[i].value = value;
	// Grow if needed
	if (map__full(h)) {
		map_grow(h);
	}
}

template <typename K, typename V>
void multi_map_remove(PtrMap<K, V> *h, K key, PtrMapEntry<K, V> *e) {
	MapFindResult fr = map__find_from_entry(h, e);
	if (fr.entry_index != MAP_SENTINEL) {
		map__erase(h, fr);
	}
}

template <typename K, typename V>
void multi_map_remove_all(PtrMap<K, V> *h, K key) {
	while (map_get(h, key) != nullptr) {
		map_remove(h, key);
	}
}
#endif
<commit_msg>Improve `ptr_map_hash_key`<commit_after>#define PTR_MAP_ENABLE_MULTI_MAP 1

typedef u32 MapIndex;

struct MapFindResult {
	MapIndex hash_index;
	MapIndex entry_prev;
	MapIndex entry_index;
};

enum : MapIndex { MAP_SENTINEL = ~(MapIndex)0 };

template <typename K, typename V>
struct PtrMapEntry {
	static_assert(sizeof(K) == sizeof(void *), "Key size must be pointer size");

	K        key;
	V        value;
	MapIndex next;
};

template <typename K, typename V>
struct PtrMap {
	Slice<MapIndex>           hashes;
	Array<PtrMapEntry<K, V> > entries;
};

u32 ptr_map_hash_key(uintptr key) {
#if defined(GB_ARCH_64_BIT)
	key = (~key) + (key << 21);
	key = key ^ (key >> 24);
	key = (key + (key << 3)) + (key << 8);
	key = key ^ (key >> 14);
	key = (key + (key << 2)) + (key << 4);
	key = key ^ (key << 28);
	return cast(u32)key;
#elif defined(GB_ARCH_32_BIT)
	u32 state = ((u32)key) * 747796405u + 2891336453u;
	u32 word = ((state >> ((state >> 28u) + 4u)) ^ state) * 277803737u;
	return (word >> 22u) ^ word;
#endif
}
u32 ptr_map_hash_key(void const *key) {
	return ptr_map_hash_key((uintptr)key);
}

template <typename K, typename V> void map_init             (PtrMap<K, V> *h, gbAllocator a, isize capacity = 16);
template <typename K, typename V> void map_destroy          (PtrMap<K, V> *h);
template <typename K, typename V> V *  map_get              (PtrMap<K, V> *h, K key);
template <typename K, typename V> void map_set              (PtrMap<K, V> *h, K key, V const &value);
template <typename K, typename V> void map_remove           (PtrMap<K, V> *h, K key);
template <typename K, typename V> void map_clear            (PtrMap<K, V> *h);
template <typename K, typename V> void map_grow             (PtrMap<K, V> *h);
template <typename K, typename V> void map_rehash           (PtrMap<K, V> *h, isize new_count);
template <typename K, typename V> void map_reserve          (PtrMap<K, V> *h, isize cap);

#if PTR_MAP_ENABLE_MULTI_MAP
// Mutlivalued map procedure
template <typename K, typename V> PtrMapEntry<K, V> * multi_map_find_first(PtrMap<K, V> *h, K key);
template <typename K, typename V> PtrMapEntry<K, V> * multi_map_find_next (PtrMap<K, V> *h, PtrMapEntry<K, V> *e);

template <typename K, typename V> isize multi_map_count     (PtrMap<K, V> *h, K key);
template <typename K, typename V> void  multi_map_get_all   (PtrMap<K, V> *h, K key, V *items);
template <typename K, typename V> void  multi_map_insert    (PtrMap<K, V> *h, K key, V const &value);
template <typename K, typename V> void  multi_map_remove    (PtrMap<K, V> *h, K key, PtrMapEntry<K, V> *e);
template <typename K, typename V> void  multi_map_remove_all(PtrMap<K, V> *h, K key);
#endif

template <typename K, typename V>
gb_inline void map_init(PtrMap<K, V> *h, gbAllocator a, isize capacity) {
	capacity = next_pow2_isize(capacity);
	slice_init(&h->hashes,  a, capacity);
	array_init(&h->entries, a, 0, capacity);
	for (isize i = 0; i < capacity; i++) {
		h->hashes.data[i] = MAP_SENTINEL;
	}
}

template <typename K, typename V>
gb_inline void map_destroy(PtrMap<K, V> *h) {
	slice_free(&h->hashes, h->entries.allocator);
	array_free(&h->entries);
}

template <typename K, typename V>
gb_internal MapIndex map__add_entry(PtrMap<K, V> *h, K key) {
	PtrMapEntry<K, V> e = {};
	e.key = key;
	e.next = MAP_SENTINEL;
	array_add(&h->entries, e);
	return cast(MapIndex)(h->entries.count-1);
}

template <typename K, typename V>
gb_internal MapFindResult map__find(PtrMap<K, V> *h, K key) {
	MapFindResult fr = {MAP_SENTINEL, MAP_SENTINEL, MAP_SENTINEL};
	if (h->hashes.count == 0) {
		return fr;
	}
	u32 hash = ptr_map_hash_key(key);
	fr.hash_index = cast(MapIndex)(hash & (h->hashes.count-1));
	fr.entry_index = h->hashes.data[fr.hash_index];
	while (fr.entry_index != MAP_SENTINEL) {
		if (h->entries.data[fr.entry_index].key == key) {
			return fr;
		}
		fr.entry_prev = fr.entry_index;
		fr.entry_index = h->entries.data[fr.entry_index].next;
	}
	return fr;
}

template <typename K, typename V>
gb_internal MapFindResult map__find_from_entry(PtrMap<K, V> *h, PtrMapEntry<K, V> *e) {
	MapFindResult fr = {MAP_SENTINEL, MAP_SENTINEL, MAP_SENTINEL};
	if (h->hashes.count == 0) {
		return fr;
	}
	u32 hash = ptr_map_hash_key(e->key);
	fr.hash_index  = cast(MapIndex)(hash & (h->hashes.count-1));
	fr.entry_index = h->hashes.data[fr.hash_index];
	while (fr.entry_index != MAP_SENTINEL) {
		if (&h->entries.data[fr.entry_index] == e) {
			return fr;
		}
		fr.entry_prev = fr.entry_index;
		fr.entry_index = h->entries.data[fr.entry_index].next;
	}
	return fr;
}

template <typename K, typename V>
gb_internal b32 map__full(PtrMap<K, V> *h) {
	return 0.75f * h->hashes.count <= h->entries.count;
}

template <typename K, typename V>
gb_inline void map_grow(PtrMap<K, V> *h) {
	isize new_count = gb_max(h->hashes.count<<1, 16);
	map_rehash(h, new_count);
}

template <typename K, typename V>
void map_reset_entries(PtrMap<K, V> *h) {
	for (isize i = 0; i < h->hashes.count; i++) {
		h->hashes.data[i] = MAP_SENTINEL;
	}
	for (isize i = 0; i < h->entries.count; i++) {
		MapFindResult fr;
		PtrMapEntry<K, V> *e = &h->entries.data[i];
		e->next = MAP_SENTINEL;
		fr = map__find_from_entry(h, e);
		if (fr.entry_prev == MAP_SENTINEL) {
			h->hashes[fr.hash_index] = cast(MapIndex)i;
		} else {
			h->entries[fr.entry_prev].next = cast(MapIndex)i;
		}
	}
}

template <typename K, typename V>
void map_reserve(PtrMap<K, V> *h, isize cap) {
	array_reserve(&h->entries, cap);
	if (h->entries.count*2 < h->hashes.count) {
		return;
	}
	slice_resize(&h->hashes, h->entries.allocator, cap*2);
	map_reset_entries(h);
}

template <typename K, typename V>
void map_rehash(PtrMap<K, V> *h, isize new_count) {
	map_reserve(h, new_count);
}

template <typename K, typename V>
V *map_get(PtrMap<K, V> *h, K key) {
	MapIndex index = map__find(h, key).entry_index;
	if (index != MAP_SENTINEL) {
		return &h->entries.data[index].value;
	}
	return nullptr;
}

template <typename K, typename V>
V &map_must_get(PtrMap<K, V> *h, K key) {
	MapIndex index = map__find(h, key).entry_index;
	GB_ASSERT(index != MAP_SENTINEL);
	return h->entries.data[index].value;
}

template <typename K, typename V>
void map_set(PtrMap<K, V> *h, K key, V const &value) {
	MapIndex index;
	MapFindResult fr;
	if (h->hashes.count == 0) {
		map_grow(h);
	}
	fr = map__find(h, key);
	if (fr.entry_index != MAP_SENTINEL) {
		index = fr.entry_index;
	} else {
		index = map__add_entry(h, key);
		if (fr.entry_prev != MAP_SENTINEL) {
			h->entries.data[fr.entry_prev].next = index;
		} else {
			h->hashes.data[fr.hash_index] = index;
		}
	}
	h->entries.data[index].value = value;

	if (map__full(h)) {
		map_grow(h);
	}
}

template <typename K, typename V>
void map__erase(PtrMap<K, V> *h, MapFindResult const &fr) {
	MapFindResult last;
	if (fr.entry_prev == MAP_SENTINEL) {
		h->hashes.data[fr.hash_index] = h->entries.data[fr.entry_index].next;
	} else {
		h->entries.data[fr.entry_prev].next = h->entries.data[fr.entry_index].next;
	}
	if (fr.entry_index == h->entries.count-1) {
		array_pop(&h->entries);
		return;
	}
	h->entries.data[fr.entry_index] = h->entries.data[h->entries.count-1];
	array_pop(&h->entries);

	last = map__find(h, h->entries.data[fr.entry_index].key);
	if (last.entry_prev != MAP_SENTINEL) {
		h->entries.data[last.entry_prev].next = fr.entry_index;
	} else {
		h->hashes.data[last.hash_index] = fr.entry_index;
	}
}

template <typename K, typename V>
void map_remove(PtrMap<K, V> *h, K key) {
	MapFindResult fr = map__find(h, key);
	if (fr.entry_index != MAP_SENTINEL) {
		map__erase(h, fr);
	}
}

template <typename K, typename V>
gb_inline void map_clear(PtrMap<K, V> *h) {
	array_clear(&h->entries);
	for (isize i = 0; i < h->hashes.count; i++) {
		h->hashes.data[i] = MAP_SENTINEL;
	}
}

#if PTR_MAP_ENABLE_MULTI_MAP
template <typename K, typename V>
PtrMapEntry<K, V> *multi_map_find_first(PtrMap<K, V> *h, K key) {
	MapIndex i = map__find(h, key).entry_index;
	if (i == MAP_SENTINEL) {
		return nullptr;
	}
	return &h->entries.data[i];
}

template <typename K, typename V>
PtrMapEntry<K, V> *multi_map_find_next(PtrMap<K, V> *h, PtrMapEntry<K, V> *e) {
	MapIndex i = e->next;
	while (i != MAP_SENTINEL) {
		if (h->entries.data[i].key == e->key) {
			return &h->entries.data[i];
		}
		i = h->entries.data[i].next;
	}
	return nullptr;
}

template <typename K, typename V>
isize multi_map_count(PtrMap<K, V> *h, K key) {
	isize count = 0;
	PtrMapEntry<K, V> *e = multi_map_find_first(h, key);
	while (e != nullptr) {
		count++;
		e = multi_map_find_next(h, e);
	}
	return count;
}

template <typename K, typename V>
void multi_map_get_all(PtrMap<K, V> *h, K key, V *items) {
	isize i = 0;
	PtrMapEntry<K, V> *e = multi_map_find_first(h, key);
	while (e != nullptr) {
		items[i++] = e->value;
		e = multi_map_find_next(h, e);
	}
}

template <typename K, typename V>
void multi_map_insert(PtrMap<K, V> *h, K key, V const &value) {
	MapFindResult fr;
	MapIndex i;
	if (h->hashes.count == 0) {
		map_grow(h);
	}
	// Make
	fr = map__find(h, key);
	i = map__add_entry(h, key);
	if (fr.entry_prev == MAP_SENTINEL) {
		h->hashes.data[fr.hash_index] = i;
	} else {
		h->entries.data[fr.entry_prev].next = i;
	}
	h->entries.data[i].next = fr.entry_index;
	h->entries.data[i].value = value;
	// Grow if needed
	if (map__full(h)) {
		map_grow(h);
	}
}

template <typename K, typename V>
void multi_map_remove(PtrMap<K, V> *h, K key, PtrMapEntry<K, V> *e) {
	MapFindResult fr = map__find_from_entry(h, e);
	if (fr.entry_index != MAP_SENTINEL) {
		map__erase(h, fr);
	}
}

template <typename K, typename V>
void multi_map_remove_all(PtrMap<K, V> *h, K key) {
	while (map_get(h, key) != nullptr) {
		map_remove(h, key);
	}
}
#endif
<|endoftext|>


<commit_before>#include "libmesh/direct_solution_transfer.h"

#include "libmesh/system.h"
#include "libmesh/numeric_vector.h"

namespace libMesh {

DirectSolutionTransfer::DirectSolutionTransfer()
{}

DirectSolutionTransfer::~DirectSolutionTransfer()
{}

void
DirectSolutionTransfer::transfer(const Variable & from_var, const Variable & to_var)
{
  System * from_sys = from_var.sys();
  System * to_sys = to_var.sys();

  // get dof indices for source variable
  unsigned int from_vn = from_var.number();
  std::set<unsigned int> from_var_indices;
  from_sys->local_dof_indices(from_vn, from_var_indices);

  // get dof indices for dest variable
  unsigned int to_vn = to_var.number();
  std::set<unsigned int> to_var_indices;
  to_sys->local_dof_indices(to_vn, to_var_indices);

  // copy the values from from solution vector to to solution vector
  std::set<unsigned int>::iterator from_it = from_var_indices.begin();
  std::set<unsigned int>::iterator from_it_end = from_var_indices.end();
  std::set<unsigned int>::iterator to_it = to_var_indices.begin();

  NumericVector<Number> & from_solution = *from_sys->solution;

  for (; from_it != from_it_end; ++from_it, ++to_it)
    to_sys->solution->set(*to_it, from_solution(*from_it));

  to_sys->solution->close();
  to_sys->update();
}

} // namespace libMesh
<commit_msg>add a couple of asserts to the DirectSolutionTransfer object... theyre not complete but theyre better than nothing<commit_after>#include "libmesh/direct_solution_transfer.h"

#include "libmesh/system.h"
#include "libmesh/numeric_vector.h"

namespace libMesh {

DirectSolutionTransfer::DirectSolutionTransfer()
{}

DirectSolutionTransfer::~DirectSolutionTransfer()
{}

void
DirectSolutionTransfer::transfer(const Variable & from_var, const Variable & to_var)
{
  System * from_sys = from_var.sys();
  System * to_sys = to_var.sys();

  // Just a couple of (not completely thorough)
  libmesh_assert(from_sys->get_equation_systems().get_mesh().n_nodes() == from_sys->get_equation_systems().get_mesh().n_nodes());
  libmesh_assert(from_var.type() == to_var.type());

  // get dof indices for source variable
  unsigned int from_vn = from_var.number();
  std::set<unsigned int> from_var_indices;
  from_sys->local_dof_indices(from_vn, from_var_indices);

  // get dof indices for dest variable
  unsigned int to_vn = to_var.number();
  std::set<unsigned int> to_var_indices;
  to_sys->local_dof_indices(to_vn, to_var_indices);

  // copy the values from from solution vector to to solution vector
  std::set<unsigned int>::iterator from_it = from_var_indices.begin();
  std::set<unsigned int>::iterator from_it_end = from_var_indices.end();
  std::set<unsigned int>::iterator to_it = to_var_indices.begin();

  NumericVector<Number> & from_solution = *from_sys->solution;

  for (; from_it != from_it_end; ++from_it, ++to_it)
    to_sys->solution->set(*to_it, from_solution(*from_it));

  to_sys->solution->close();
  to_sys->update();
}

} // namespace libMesh
<|endoftext|>


<commit_before>#ifndef STAN__MATH__MATRIX_SPARSE_EXTRACTORS_HPP
#define STAN__MATH__MATRIX_SPARSE_EXTRACTORS_HPP

#include <Eigen/Sparse>
#include <vector>
#include <numeric>

namespace stan {

  namespace math {
    // FIXME: The implementations are the same, only the interpretation
    // differs in CSC vs. CSR.  In the time I had I couldn't get one
    // implementation that swallowed both matrix types.

    /** @defgroup sparse_csc CSC Sparse Extractors.
     *  This group of functions extracts the components of a
     *  Compressed Sparse Column (CSC) sparse matrix.  The components
     *  are:
     *    - w: the non-zero values in the sparse matrix.
     *    - v: one-based row index for each value in w, as a result this
     *      is the same length as w.
     *    - u: one-based index of where each column starts in w, length
     *      is equal to the number of columns plus one.  Last entry is
     *      one-past-the-end in w (one-based...)
     *    - z: number of non-zero entries in each column of w, length is
     *      equal to the number of columns.
     *  @{
     */

    template <typename T>
    const Eigen::Matrix<T, Eigen::Dynamic,1> extract_w(Eigen::SparseMatrix<T> A) {
      Eigen::Matrix<T,Eigen::Dynamic,1> w(A.nonZeros());
      for (int j = 0; j < A.nonZeros(); ++j)
        w[j] = *(A.valuePtr()+j);
      return w;
    }

    template <typename T>
    const std::vector<int> extract_v(Eigen::SparseMatrix<T> A) {
      std::vector<int> v(A.nonZeros());
      for (int j = 0; j < A.nonZeros(); ++j)
        v[j] = *(A.innerIndexPtr()+j) + 1;   // make 1-indexed
      return v;
    }

    template <typename T>
    const std::vector<int> extract_u(Eigen::SparseMatrix<T> A) {
      std::vector<int> u(A.outerSize()+1);
      for (int j = 0; j <= A.outerSize(); ++j)
        u[j] = *(A.outerIndexPtr()+j) + 1; // make 1-indexed
      return u;
    }

    template <typename T>
    const std::vector<int> extract_z(Eigen::SparseMatrix<T> A) {
      std::vector<int> u(A.outerSize()+1);
      std::vector<int> z(A.outerSize()+1);
      u = extract_u(A);
      std::adjacent_difference(u.begin(), u.end(), z.begin());
      z.erase(z.begin());
      return z;
    }
    /** @} */ // end of sparse_csc group.

    /** @defgroup sparse_csr CSR Sparse Extractors.
     *  This group of functions extracts the components of a
     *  Compressed Sparse Row (CSR) sparse matrix.  The components
     *  are:
     *    - w: the non-zero values in the sparse matrix.
     *    - v: one-based column index for each value in w, as a result this
     *      is the same length as w.
     *    - u: one-based index of where each row starts in w, length
     *      is equal to the number of rows plus one.  Last entry is
     *      one-past-the-end in w (one-based...)
     *    - z: number of non-zero entries in each row of w, length is
     *      equal to the number of rows.
     *  @{
     */
    template <typename T>
    const Eigen::Matrix<T,Eigen::Dynamic,1> extract_w(Eigen::SparseMatrix<T, Eigen::RowMajor> A) {
      Eigen::Matrix<T,Eigen::Dynamic,1> w(A.nonZeros());
      for (int j = 0; j < A.nonZeros(); ++j)
        w[j] = *(A.valuePtr()+j);
      return w;
    }

    template <typename T>
    const std::vector<int> extract_v(Eigen::SparseMatrix<T, Eigen::RowMajor> A) {
      std::vector<int> v(A.nonZeros());
      for (int j = 0; j < A.nonZeros(); ++j)
        v[j] = *(A.innerIndexPtr()+j) + 1;  // make 1-indexed
      return v;
    }

    template <typename T>
    const std::vector<int> extract_u(Eigen::SparseMatrix<T, Eigen::RowMajor> A) {
      std::vector<int> u(A.outerSize()+1);
      for (int j = 0; j <= A.outerSize(); ++j)
        u[j] = *(A.outerIndexPtr()+j) + 1;  // make 1-indexed
      return u;
    }

    template <typename T>
    const std::vector<int> extract_z(Eigen::SparseMatrix<T, Eigen::RowMajor> A) {
      std::vector<int> u(A.outerSize()+1);
      std::vector<int> z(A.outerSize()+1);
      u = extract_u(A);
      std::adjacent_difference(u.begin(), u.end(), z.begin());
      z.erase(z.begin());
      return z;
    }

    /** @} */ // end of sparse_csr group.
  }
}

#endif
<commit_msg>Added setZero() to sparse_extractors. AFAIK not needed on std::vector<int> as initializing them to a length sets all entries to zero.<commit_after>#ifndef STAN__MATH__MATRIX_SPARSE_EXTRACTORS_HPP
#define STAN__MATH__MATRIX_SPARSE_EXTRACTORS_HPP

#include <Eigen/Sparse>
#include <vector>
#include <numeric>

namespace stan {

  namespace math {
    // FIXME: The implementations are the same, only the interpretation
    // differs in CSC vs. CSR.  In the time I had I couldn't get one
    // implementation that swallowed both matrix types.

    /** @defgroup sparse_csc CSC Sparse Extractors.
     *  This group of functions extracts the components of a
     *  Compressed Sparse Column (CSC) sparse matrix.  The components
     *  are:
     *    - w: the non-zero values in the sparse matrix.
     *    - v: one-based row index for each value in w, as a result this
     *      is the same length as w.
     *    - u: one-based index of where each column starts in w, length
     *      is equal to the number of columns plus one.  Last entry is
     *      one-past-the-end in w (one-based...)
     *    - z: number of non-zero entries in each column of w, length is
     *      equal to the number of columns.
     *  @{
     */

    template <typename T>
    const Eigen::Matrix<T, Eigen::Dynamic,1> extract_w(Eigen::SparseMatrix<T> A) {
      Eigen::Matrix<T,Eigen::Dynamic,1> w(A.nonZeros());
      w.setZero();
      for (int j = 0; j < A.nonZeros(); ++j)
        w[j] = *(A.valuePtr()+j);
      return w;
    }

    template <typename T>
    const std::vector<int> extract_v(Eigen::SparseMatrix<T> A) {
      std::vector<int> v(A.nonZeros());
      for (int j = 0; j < A.nonZeros(); ++j)
        v[j] = *(A.innerIndexPtr()+j) + 1;   // make 1-indexed
      return v;
    }

    template <typename T>
    const std::vector<int> extract_u(Eigen::SparseMatrix<T> A) {
      std::vector<int> u(A.outerSize()+1);
      for (int j = 0; j <= A.outerSize(); ++j)
        u[j] = *(A.outerIndexPtr()+j) + 1; // make 1-indexed
      return u;
    }

    template <typename T>
    const std::vector<int> extract_z(Eigen::SparseMatrix<T> A) {
      std::vector<int> u(A.outerSize()+1);
      std::vector<int> z(A.outerSize()+1);
      u = extract_u(A);
      std::adjacent_difference(u.begin(), u.end(), z.begin());
      z.erase(z.begin());
      return z;
    }
    /** @} */ // end of sparse_csc group.

    /** @defgroup sparse_csr CSR Sparse Extractors.
     *  This group of functions extracts the components of a
     *  Compressed Sparse Row (CSR) sparse matrix.  The components
     *  are:
     *    - w: the non-zero values in the sparse matrix.
     *    - v: one-based column index for each value in w, as a result this
     *      is the same length as w.
     *    - u: one-based index of where each row starts in w, length
     *      is equal to the number of rows plus one.  Last entry is
     *      one-past-the-end in w (one-based...)
     *    - z: number of non-zero entries in each row of w, length is
     *      equal to the number of rows.
     *  @{
     */
    template <typename T>
    const Eigen::Matrix<T,Eigen::Dynamic,1> extract_w(Eigen::SparseMatrix<T, Eigen::RowMajor> A) {
      Eigen::Matrix<T,Eigen::Dynamic,1> w(A.nonZeros());
      w.setZero();
      for (int j = 0; j < A.nonZeros(); ++j)
        w[j] = *(A.valuePtr()+j);
      return w;
    }

    template <typename T>
    const std::vector<int> extract_v(Eigen::SparseMatrix<T, Eigen::RowMajor> A) {
      std::vector<int> v(A.nonZeros());
      for (int j = 0; j < A.nonZeros(); ++j)
        v[j] = *(A.innerIndexPtr()+j) + 1;  // make 1-indexed
      return v;
    }

    template <typename T>
    const std::vector<int> extract_u(Eigen::SparseMatrix<T, Eigen::RowMajor> A) {
      std::vector<int> u(A.outerSize()+1);
      for (int j = 0; j <= A.outerSize(); ++j)
        u[j] = *(A.outerIndexPtr()+j) + 1;  // make 1-indexed
      return u;
    }

    template <typename T>
    const std::vector<int> extract_z(Eigen::SparseMatrix<T, Eigen::RowMajor> A) {
      std::vector<int> u(A.outerSize()+1);
      std::vector<int> z(A.outerSize()+1);
      u = extract_u(A);
      std::adjacent_difference(u.begin(), u.end(), z.begin());
      z.erase(z.begin());
      return z;
    }

    /** @} */ // end of sparse_csr group.
  }
}

#endif
<|endoftext|>


<commit_before>#include "animation_editor.h"
#include "animation_inputs.h"
#include "animator.h"
#include "mainwindow.h"
#include "property_view/property_editor.h"
#include "property_view.h"
#include "scripts/scriptcompiler.h"
#include "skeleton_view.h"
#include <qaction.h>
#include <qevent.h>
#include <qlayout.h>
#include <qmenu.h>
#include <qmenubar.h>
#include <qmessagebox.h>
#include <qpainter.h>
#include <qtoolbar.h>

static const QColor GRID_COLOR(60, 60, 60);
static const QColor EDGE_COLOR(255, 255, 255);
static const int GRID_CELL_SIZE = 32;
static QLinearGradient ANIMATION_NODE_GRADIENT(0, 0, 0, 100);

AnimationEditor::AnimationEditor(MainWindow& main_window)
	: m_property_view(*main_window.getPropertyView())
	, m_compiler(*main_window.getScriptCompiler())
	, m_main_window(main_window)
{
	m_animator = new Animator(m_compiler);
	emit animatorCreated();

	setWindowTitle("Animation editor");
	setObjectName("animationEditor");
	QWidget* widget = new QWidget(this);
	QVBoxLayout* layout = new QVBoxLayout(widget);
	layout->setContentsMargins(0, 0, 0, 0);
	m_animation_graph_view = new AnimationGraphView(*this);
	setWidget(widget);
	QToolBar* toolbar = new QToolBar(widget);
	m_compile_action = toolbar->addAction("Compile");
	m_run_action = toolbar->addAction("Run");
	m_save_action = toolbar->addAction("Save");
	m_save_as_action = toolbar->addAction("Save As");
	m_load_action = toolbar->addAction("Load");
	connect(m_compile_action, &QAction::triggered, this, &AnimationEditor::onCompileAction);
	connect(m_run_action, &QAction::triggered, this, &AnimationEditor::onRunAction);
	connect(m_save_action, &QAction::triggered, this, &AnimationEditor::onSaveAction);
	connect(m_save_as_action, &QAction::triggered, this, &AnimationEditor::onSaveAsAction);
	connect(m_load_action, &QAction::triggered, this, &AnimationEditor::onLoadAction);
	layout->addWidget(toolbar);
	layout->addWidget(m_animation_graph_view);

	m_inputs = new AnimationInputs(*this);
	m_skeleton_view = new SkeletonView();

	addMenu(main_window);

	ANIMATION_NODE_GRADIENT.setColorAt(0.0, QColor(0, 255, 0, 128));
	ANIMATION_NODE_GRADIENT.setColorAt(1.0, QColor(0, 64, 0, 128));
	ANIMATION_NODE_GRADIENT.setSpread(QGradient::Spread::ReflectSpread);
}

void AnimationEditor::addMenu(MainWindow& main_window)
{
	QMenuBar* menu_bar = main_window.getMenuBar();
	QMenu* menu = menu_bar->addMenu("Animation Editor");
	menu->addAction(m_compile_action);
	menu->addAction(m_run_action);
	menu->addAction(m_load_action);
	menu->addAction(m_save_action);
	menu->addAction(m_save_as_action);
	m_view_menu = menu->addMenu("View");
	addEditorDock(Qt::DockWidgetArea::BottomDockWidgetArea, this);
	addEditorDock(Qt::DockWidgetArea::BottomDockWidgetArea, m_inputs);
	addEditorDock(Qt::DockWidgetArea::BottomDockWidgetArea, m_skeleton_view);
	m_view_menu->connect(m_view_menu, &QMenu::aboutToShow, [this]() {
		for (auto info : m_dock_infos)
		{
			info.m_action->setChecked(info.m_widget->isVisible());
		}
	});
}

void AnimationEditor::addEditorDock(Qt::DockWidgetArea area, QDockWidget* widget)
{
	DockInfo info;
	info.m_widget = widget;
	QAction* action = widget->toggleViewAction();
	action->setCheckable(true);
	m_view_menu->addAction(action);
	info.m_action = action;
	action->connect(action, &QAction::triggered, this, [widget](){
		widget->show();
	});
	m_dock_infos.push_back(info);
	m_main_window.addDockWidget(area, widget);
}

void AnimationEditor::onCompileAction()
{
	if (!m_animator->isValidPath())
	{
		onSaveAsAction();
		if (!m_animator->isValidPath())
		{
			return;
		}
	}
	m_animator->compile();
}

void AnimationEditor::onRunAction()
{
	m_animator->run();
}

void AnimationEditor::onSaveAsAction()
{
	QString path = QFileDialog::getSaveFileName(NULL, QString(), QString(), "All files (*.grf)");
	if (!path.isEmpty())
	{
		m_animator->setPath(path);
		onSaveAction();
	}
}

void AnimationEditor::onSaveAction()
{
	if (!m_animator->isValidPath())
	{
		QString path = QFileDialog::getSaveFileName(NULL, QString(), QString(), "All files (*.grf)");
		if (!path.isEmpty())
		{
			m_animator->setPath(path);
		}
		else
		{
			return;
		}
	}

	Lumix::OutputBlob blob(m_editor->getEngine().getAllocator());
	m_animator->serialize(blob);
	QFile file(m_animator->getPath());
	file.open(QIODevice::WriteOnly);
	file.write((const char*)blob.getData(), blob.getSize());
	file.close();
}

void AnimationEditor::onLoadAction()
{
	QString path = QFileDialog::getOpenFileName(NULL, QString(), QString(), "All files (*.grf)");
	if (!path.isEmpty())
	{
		delete m_animator;
		m_animator = new Animator(m_compiler);
		m_animator->setPath(path);
		m_animator->setWorldEditor(*m_editor);
		QFile file(path);
		file.open(QIODevice::ReadOnly);
		QByteArray data = file.readAll();

		Lumix::InputBlob blob(data.data(), data.size());
		m_animator->deserialize(*this, blob);
		m_animation_graph_view->setNode(m_animator->getRoot());
		emit animatorCreated();
	}
}

AnimatorNodeContent* AnimationEditor::createContent(AnimatorNode& node, uint32_t content_type)
{
	TODO("todo");
	if (content_type == crc32("animation"))
	{
		return new AnimationNodeContent(&node);
	}
	else if (content_type == crc32("state_machine"))
	{
		return new StateMachineNodeContent(&node);
	}
	return NULL;
}

void AnimationEditor::setWorldEditor(Lumix::WorldEditor& editor)
{
	m_editor = &editor;
	m_animator->setWorldEditor(editor);
	m_skeleton_view->setWorldEditor(editor);
}

void AnimationEditor::setComponent(const Lumix::Component&)
{

}

void AnimationEditor::executeCommand(QUndoCommand* command)
{
	m_undo_stack.push(command);
}

void AnimationEditor::update(float time_delta)
{
	m_animator->update(time_delta);
}

AnimationGraphView::AnimationGraphView(AnimationEditor& editor)
	: QWidget(&editor)
	, m_editor(editor)
{
	m_node = editor.getAnimator()->getRoot();
	m_mouse_mode = MouseMode::NONE;
	m_mouse_node = NULL;
	setContextMenuPolicy(Qt::CustomContextMenu);
	connect(this, &QWidget::customContextMenuRequested, this, &AnimationGraphView::showContextMenu);
}

void AnimationGraphView::showContextMenu(const QPoint& pos)
{
	if (m_mouse_mode != MouseMode::EDGE)
	{
		m_node->showContextMenu(m_editor, this, pos);
	}
	m_mouse_mode = MouseMode::NONE;
	update();
}

void AnimationGraphView::paintEvent(QPaintEvent*)
{
	QPainter painter(this);
	drawGrid(painter);
	drawNewEdge(painter);
	drawNodes(painter);
}

void AnimationGraphView::selectEdge(AnimatorEdge* edge)
{
	edge->fillPropertyView(m_editor.getPropertyView());
}

void AnimationGraphView::selectNode(AnimatorNode* node)
{
	node->getContent()->fillPropertyView(m_editor.getPropertyView());
}

void AnimationGraphView::mouseReleaseEvent(QMouseEvent* event)
{
	AnimatorNode* node = m_node->getContentNodeAt(event->x(), event->y());
	if (m_mouse_mode == MouseMode::EDGE && node && m_node != node && node != m_mouse_node)
	{
		if (m_node->getContent()->getType() == crc32("state_machine"))
		{
			static_cast<StateMachineNodeContent*>(m_node->getContent())->createEdge(*m_editor.getAnimator(), m_mouse_node, node);
		}
	}
	else
	{
		m_mouse_mode = MouseMode::NONE;
	}

	if (event->button() != Qt::RightButton)
	{
		m_mouse_mode = MouseMode::NONE;
	}
}

void AnimationGraphView::mousePressEvent(QMouseEvent* event)
{
	AnimatorNode* node = m_node->getContentNodeAt(event->x(), event->y());
	if (node && m_node != node)
	{
		m_mouse_mode = event->button() == Qt::RightButton ? MouseMode::EDGE : MouseMode::DRAGGING;
		m_mouse_node = node;
		m_last_mouse_position.setX(event->x());
		m_last_mouse_position.setY(event->y());
		selectNode(node);
	}
	else if (m_node == node)
	{
		AnimatorEdge* edge = m_node->getContent()->getEdgeAt(event->x(), event->y());
		if (edge)
		{
			selectEdge(edge);
		}

	}
}

void AnimationGraphView::mouseMoveEvent(QMouseEvent* event)
{
	if (m_mouse_mode == MouseMode::DRAGGING)
	{
		QPoint new_position = m_mouse_node->getPosition();
		new_position.setX(new_position.x() + event->x() - m_last_mouse_position.x());
		new_position.setY(new_position.y() + event->y() - m_last_mouse_position.y());
		m_mouse_node->setPosition(new_position);
		update();
	}
	else if (m_mouse_mode == MouseMode::EDGE)
	{
		update();
	}
	m_last_mouse_position.setX(event->x());
	m_last_mouse_position.setY(event->y());
}

void AnimationGraphView::drawGrid(QPainter& painter)
{
	painter.setPen(GRID_COLOR);

	for (int i = 0; i < height() / GRID_CELL_SIZE; ++i)
	{
		painter.drawLine(0, i * GRID_CELL_SIZE, width(), i * GRID_CELL_SIZE);
	}

	for (int i = 0; i < width() / GRID_CELL_SIZE; ++i)
	{
		painter.drawLine(i * GRID_CELL_SIZE, 0, i * GRID_CELL_SIZE, height());
	}
}

void AnimationGraphView::drawNewEdge(QPainter& painter)
{
	if (m_mouse_mode == MouseMode::EDGE)
	{
		painter.setPen(EDGE_COLOR);
		painter.drawLine(m_last_mouse_position, m_mouse_node->getCenter());
	}
}

void AnimationGraphView::drawNodes(QPainter& painter)
{
	if (m_node)
	{
		m_node->paintContent(painter);
	}
}
<commit_msg>todo fixed<commit_after>#include "animation_editor.h"
#include "animation_inputs.h"
#include "animator.h"
#include "mainwindow.h"
#include "property_view/property_editor.h"
#include "property_view.h"
#include "scripts/scriptcompiler.h"
#include "skeleton_view.h"
#include <qaction.h>
#include <qevent.h>
#include <qlayout.h>
#include <qmenu.h>
#include <qmenubar.h>
#include <qmessagebox.h>
#include <qpainter.h>
#include <qtoolbar.h>

static const QColor GRID_COLOR(60, 60, 60);
static const QColor EDGE_COLOR(255, 255, 255);
static const int GRID_CELL_SIZE = 32;
static QLinearGradient ANIMATION_NODE_GRADIENT(0, 0, 0, 100);

AnimationEditor::AnimationEditor(MainWindow& main_window)
	: m_property_view(*main_window.getPropertyView())
	, m_compiler(*main_window.getScriptCompiler())
	, m_main_window(main_window)
{
	m_animator = new Animator(m_compiler);
	emit animatorCreated();

	setWindowTitle("Animation editor");
	setObjectName("animationEditor");
	QWidget* widget = new QWidget(this);
	QVBoxLayout* layout = new QVBoxLayout(widget);
	layout->setContentsMargins(0, 0, 0, 0);
	m_animation_graph_view = new AnimationGraphView(*this);
	setWidget(widget);
	QToolBar* toolbar = new QToolBar(widget);
	m_compile_action = toolbar->addAction("Compile");
	m_run_action = toolbar->addAction("Run");
	m_save_action = toolbar->addAction("Save");
	m_save_as_action = toolbar->addAction("Save As");
	m_load_action = toolbar->addAction("Load");
	connect(m_compile_action, &QAction::triggered, this, &AnimationEditor::onCompileAction);
	connect(m_run_action, &QAction::triggered, this, &AnimationEditor::onRunAction);
	connect(m_save_action, &QAction::triggered, this, &AnimationEditor::onSaveAction);
	connect(m_save_as_action, &QAction::triggered, this, &AnimationEditor::onSaveAsAction);
	connect(m_load_action, &QAction::triggered, this, &AnimationEditor::onLoadAction);
	layout->addWidget(toolbar);
	layout->addWidget(m_animation_graph_view);

	m_inputs = new AnimationInputs(*this);
	m_skeleton_view = new SkeletonView();

	addMenu(main_window);

	ANIMATION_NODE_GRADIENT.setColorAt(0.0, QColor(0, 255, 0, 128));
	ANIMATION_NODE_GRADIENT.setColorAt(1.0, QColor(0, 64, 0, 128));
	ANIMATION_NODE_GRADIENT.setSpread(QGradient::Spread::ReflectSpread);
}

void AnimationEditor::addMenu(MainWindow& main_window)
{
	QMenuBar* menu_bar = main_window.getMenuBar();
	QMenu* menu = menu_bar->addMenu("Animation Editor");
	menu->addAction(m_compile_action);
	menu->addAction(m_run_action);
	menu->addAction(m_load_action);
	menu->addAction(m_save_action);
	menu->addAction(m_save_as_action);
	m_view_menu = menu->addMenu("View");
	addEditorDock(Qt::DockWidgetArea::BottomDockWidgetArea, this);
	addEditorDock(Qt::DockWidgetArea::BottomDockWidgetArea, m_inputs);
	addEditorDock(Qt::DockWidgetArea::BottomDockWidgetArea, m_skeleton_view);
	m_view_menu->connect(m_view_menu, &QMenu::aboutToShow, [this]() {
		for (auto info : m_dock_infos)
		{
			info.m_action->setChecked(info.m_widget->isVisible());
		}
	});
}

void AnimationEditor::addEditorDock(Qt::DockWidgetArea area, QDockWidget* widget)
{
	DockInfo info;
	info.m_widget = widget;
	QAction* action = widget->toggleViewAction();
	action->setCheckable(true);
	m_view_menu->addAction(action);
	info.m_action = action;
	action->connect(action, &QAction::triggered, this, [widget](){
		widget->show();
	});
	m_dock_infos.push_back(info);
	m_main_window.addDockWidget(area, widget);
}

void AnimationEditor::onCompileAction()
{
	if (!m_animator->isValidPath())
	{
		onSaveAsAction();
		if (!m_animator->isValidPath())
		{
			return;
		}
	}
	m_animator->compile();
}

void AnimationEditor::onRunAction()
{
	m_animator->run();
}

void AnimationEditor::onSaveAsAction()
{
	QString path = QFileDialog::getSaveFileName(NULL, QString(), QString(), "All files (*.grf)");
	if (!path.isEmpty())
	{
		m_animator->setPath(path);
		onSaveAction();
	}
}

void AnimationEditor::onSaveAction()
{
	if (!m_animator->isValidPath())
	{
		QString path = QFileDialog::getSaveFileName(NULL, QString(), QString(), "All files (*.grf)");
		if (!path.isEmpty())
		{
			m_animator->setPath(path);
		}
		else
		{
			return;
		}
	}

	Lumix::OutputBlob blob(m_editor->getEngine().getAllocator());
	m_animator->serialize(blob);
	QFile file(m_animator->getPath());
	file.open(QIODevice::WriteOnly);
	file.write((const char*)blob.getData(), blob.getSize());
	file.close();
}

void AnimationEditor::onLoadAction()
{
	QString path = QFileDialog::getOpenFileName(NULL, QString(), QString(), "All files (*.grf)");
	if (!path.isEmpty())
	{
		delete m_animator;
		m_animator = new Animator(m_compiler);
		m_animator->setPath(path);
		m_animator->setWorldEditor(*m_editor);
		QFile file(path);
		file.open(QIODevice::ReadOnly);
		QByteArray data = file.readAll();

		Lumix::InputBlob blob(data.data(), data.size());
		m_animator->deserialize(*this, blob);
		m_animation_graph_view->setNode(m_animator->getRoot());
		emit animatorCreated();
	}
}

AnimatorNodeContent* AnimationEditor::createContent(AnimatorNode& node, uint32_t content_type)
{
	static const uint32_t ANIMATION_HASH = crc32("animation");
	static const uint32_t STATE_MACHINE_HASH = crc32("state_machine");
	if (content_type == ANIMATION_HASH)
	{
		return new AnimationNodeContent(&node);
	}
	else if (content_type == STATE_MACHINE_HASH)
	{
		return new StateMachineNodeContent(&node);
	}
	return NULL;
}

void AnimationEditor::setWorldEditor(Lumix::WorldEditor& editor)
{
	m_editor = &editor;
	m_animator->setWorldEditor(editor);
	m_skeleton_view->setWorldEditor(editor);
}

void AnimationEditor::setComponent(const Lumix::Component&)
{

}

void AnimationEditor::executeCommand(QUndoCommand* command)
{
	m_undo_stack.push(command);
}

void AnimationEditor::update(float time_delta)
{
	m_animator->update(time_delta);
}

AnimationGraphView::AnimationGraphView(AnimationEditor& editor)
	: QWidget(&editor)
	, m_editor(editor)
{
	m_node = editor.getAnimator()->getRoot();
	m_mouse_mode = MouseMode::NONE;
	m_mouse_node = NULL;
	setContextMenuPolicy(Qt::CustomContextMenu);
	connect(this, &QWidget::customContextMenuRequested, this, &AnimationGraphView::showContextMenu);
}

void AnimationGraphView::showContextMenu(const QPoint& pos)
{
	if (m_mouse_mode != MouseMode::EDGE)
	{
		m_node->showContextMenu(m_editor, this, pos);
	}
	m_mouse_mode = MouseMode::NONE;
	update();
}

void AnimationGraphView::paintEvent(QPaintEvent*)
{
	QPainter painter(this);
	drawGrid(painter);
	drawNewEdge(painter);
	drawNodes(painter);
}

void AnimationGraphView::selectEdge(AnimatorEdge* edge)
{
	edge->fillPropertyView(m_editor.getPropertyView());
}

void AnimationGraphView::selectNode(AnimatorNode* node)
{
	node->getContent()->fillPropertyView(m_editor.getPropertyView());
}

void AnimationGraphView::mouseReleaseEvent(QMouseEvent* event)
{
	AnimatorNode* node = m_node->getContentNodeAt(event->x(), event->y());
	if (m_mouse_mode == MouseMode::EDGE && node && m_node != node && node != m_mouse_node)
	{
		if (m_node->getContent()->getType() == crc32("state_machine"))
		{
			static_cast<StateMachineNodeContent*>(m_node->getContent())->createEdge(*m_editor.getAnimator(), m_mouse_node, node);
		}
	}
	else
	{
		m_mouse_mode = MouseMode::NONE;
	}

	if (event->button() != Qt::RightButton)
	{
		m_mouse_mode = MouseMode::NONE;
	}
}

void AnimationGraphView::mousePressEvent(QMouseEvent* event)
{
	AnimatorNode* node = m_node->getContentNodeAt(event->x(), event->y());
	if (node && m_node != node)
	{
		m_mouse_mode = event->button() == Qt::RightButton ? MouseMode::EDGE : MouseMode::DRAGGING;
		m_mouse_node = node;
		m_last_mouse_position.setX(event->x());
		m_last_mouse_position.setY(event->y());
		selectNode(node);
	}
	else if (m_node == node)
	{
		AnimatorEdge* edge = m_node->getContent()->getEdgeAt(event->x(), event->y());
		if (edge)
		{
			selectEdge(edge);
		}

	}
}

void AnimationGraphView::mouseMoveEvent(QMouseEvent* event)
{
	if (m_mouse_mode == MouseMode::DRAGGING)
	{
		QPoint new_position = m_mouse_node->getPosition();
		new_position.setX(new_position.x() + event->x() - m_last_mouse_position.x());
		new_position.setY(new_position.y() + event->y() - m_last_mouse_position.y());
		m_mouse_node->setPosition(new_position);
		update();
	}
	else if (m_mouse_mode == MouseMode::EDGE)
	{
		update();
	}
	m_last_mouse_position.setX(event->x());
	m_last_mouse_position.setY(event->y());
}

void AnimationGraphView::drawGrid(QPainter& painter)
{
	painter.setPen(GRID_COLOR);

	for (int i = 0; i < height() / GRID_CELL_SIZE; ++i)
	{
		painter.drawLine(0, i * GRID_CELL_SIZE, width(), i * GRID_CELL_SIZE);
	}

	for (int i = 0; i < width() / GRID_CELL_SIZE; ++i)
	{
		painter.drawLine(i * GRID_CELL_SIZE, 0, i * GRID_CELL_SIZE, height());
	}
}

void AnimationGraphView::drawNewEdge(QPainter& painter)
{
	if (m_mouse_mode == MouseMode::EDGE)
	{
		painter.setPen(EDGE_COLOR);
		painter.drawLine(m_last_mouse_position, m_mouse_node->getCenter());
	}
}

void AnimationGraphView::drawNodes(QPainter& painter)
{
	if (m_node)
	{
		m_node->paintContent(painter);
	}
}
<|endoftext|>


<commit_before>/*
 * fMBT, free Model Based Testing tool
 * Copyright (c) 2011, Intel Corporation.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU Lesser General Public License,
 * version 2.1, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
 * more details.
 *
 * You should have received a copy of the GNU Lesser General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
 *
 */
#define _RANDOM_INTERNAL_
#include "function.hh"
#include "random_c.hh"

Random_C::Random_C(const std::string& param) {
  max_val = RAND_MAX;
  if (param=="") {
    global=true;
    single=true;
  } else {
    Function* f=new_function(param);
    if (f==NULL) {
      status=false;
      errormsg="Can't create function "+param;
      return;
    }

    if (!f->status) {
      status=false;
      errormsg="Function error:"+f->errormsg;
      return;
    }

    seed = f->val();
    global=false;
    delete f;
  }
}

unsigned long Random_C::rand() {
  if (global) {
    return rand();
  }
  return rand_r(&seed);
}

FACTORY_DEFAULT_CREATOR(Random, Random_C, "C")
<commit_msg>Doh\!<commit_after>/*
 * fMBT, free Model Based Testing tool
 * Copyright (c) 2011, Intel Corporation.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU Lesser General Public License,
 * version 2.1, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
 * more details.
 *
 * You should have received a copy of the GNU Lesser General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
 *
 */
#define _RANDOM_INTERNAL_
#include "function.hh"
#include "random_c.hh"
#include <cstdlib>

Random_C::Random_C(const std::string& param) {
  max_val = RAND_MAX;
  if (param=="") {
    global=true;
    single=true;
  } else {
    Function* f=new_function(param);
    if (f==NULL) {
      status=false;
      errormsg="Can't create function "+param;
      return;
    }

    if (!f->status) {
      status=false;
      errormsg="Function error:"+f->errormsg;
      return;
    }

    seed = f->val();
    global=false;
    delete f;
  }
}

unsigned long Random_C::rand() {
  if (global) {
    return std::rand();
  }
  return rand_r(&seed);
}

FACTORY_DEFAULT_CREATOR(Random, Random_C, "C")
<|endoftext|>


<commit_before>#include "raycast.h"
#include <iostream>
#include <glm/gtx/string_cast.hpp>
#undef near
#undef far
using std::unique_ptr;
tnw::Raycast::Raycast(std::vector<unique_ptr<Model>>& _models, IsometricCamera &_camera, size_t _width, size_t _height, GLuint _texId) :
	models(_models), camera(_camera), width(_width), height(_height), texId(_texId), image(width, height)
{
	glBindTexture(GL_TEXTURE_2D, texId);

	rays.resize(width);
	for (size_t i = 0; i < width; i++) {
		rays[i].reserve(height);
	}
	for (size_t j = 0; j < height; j++) {
		for (size_t i = 0; i < width; i++) {
			image(i,j) = std::make_tuple(0,0,1);
		}
	}

	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_FLOAT, image);
	// std::cout << "================================================\n";
	generateRays();
	paintImage();
	// std::cout << "================================================\n";
}

void tnw::Raycast::generateRays() {
	auto dx = 2.0/width,
		 dy = 2.0/height;
	for (size_t j = 0; j < height; ++j) {
		for (size_t i = 0; i < width; ++i) {

			auto x = -2.f*(1.f/2.f - float(i)/width);
			auto y = 2.f*(1.f/2.f - float(j)/height);

			float pospixx = x+dx/2;
			float pospixy = y-dy/2;

			// std::cout << "untransf a: " << glm::to_string(glm::vec3(pospixx, pospixy, camera.near)) << " unstransf b: " << glm::to_string(glm::vec3(pospixx, pospixy, camera.far)) << std::endl;

			auto cameraMatrix = glm::mat4(1/camera.scale);
			cameraMatrix = glm::translate(glm::mat4(),-camera.pos) * cameraMatrix;
			cameraMatrix = glm::inverse(tnw::isometric(camera.aspect, -camera.near, -camera.far, camera.positive_hor, camera.positive_ver)) * cameraMatrix;

			// auto aM = glm::rotate(glm::mat4(), glm::radians(-45.0f), glm::vec3(0.0,1.0,0.0));
			// auto bM = glm::rotate(glm::mat4(), -std::asin(std::tan(glm::radians(30.0f))), glm::vec3(1.0,0.0,0.0));
			// cameraMatrix = aM*bM*cameraMatrix;

			glm::vec4 a = cameraMatrix * glm::vec4(pospixx, pospixy, camera.near, 1.0f);
			glm::vec4 b = cameraMatrix * glm::vec4(pospixx, pospixy, camera.far, 1.0f);

			// std::cout << "i: " << i << " j: " << j << "\n";
			// std::cout << "a: " << glm::to_string(a) << "b: " << glm::to_string(b) << std::endl;

			// tnw::Ray r = tnw::Ray(a,b);
			// rays[i][j] = r;
			rays[i].push_back(tnw::Ray(glm::vec3(a),glm::vec3(b)));
		}
	}
}

void tnw::Raycast::paintImage() {

	for (size_t j = 0; j < height; ++j) {
		for (size_t i = 0; i < width; ++i) {
			PaintColor paintColor = {0.0, 0.0, 0.0};
			float maxDist = glm::length(rays[i][j].dir);
			// std::cout << "maxDist: " << maxDist << "\n";
			std::tuple<tnw::Color, float> minInter = std::make_tuple(tnw::Color::white, maxDist);
			// printf("%zu,%zu\n",i,j);
			for (size_t k = 0; k < models.size(); k++) {
				tnw::IntersectionList ilist = models[k]->intersect_ray(rays[i][j]);

				assert(ilist.size() > 0);
				switch (std::get<0>(ilist[0])) {
					case tnw::Color::black:
					case tnw::Color::gray:
						if (std::get<1>(minInter) > std::get<1>(ilist[0])) {
							minInter = ilist[0];
							paintColor = models[k]->getColor();
						}
						break;
					case tnw::Color::white:
						if (std::get<1>(ilist[0]) < maxDist && std::get<1>(minInter) > std::get<1>(ilist[0])) {
							minInter = ilist[0];
							paintColor = models[k]->getColor();
						}
				}
			}
			image(i,j) = std::make_tuple(paintColor[0], paintColor[1], paintColor[2]);
		}
	}

	glTexSubImage2D(GL_TEXTURE_2D, 0 ,0, 0, width, height, GL_RGB, GL_FLOAT, image);
}
<commit_msg>Mudando raycast para não desenhar modelos invisiveis<commit_after>#include "raycast.h"
#include <iostream>
#include <glm/gtx/string_cast.hpp>
#undef near
#undef far
using std::unique_ptr;
tnw::Raycast::Raycast(std::vector<unique_ptr<Model>>& _models, IsometricCamera &_camera, size_t _width, size_t _height, GLuint _texId) :
	models(_models), camera(_camera), width(_width), height(_height), texId(_texId), image(width, height)
{
	glBindTexture(GL_TEXTURE_2D, texId);

	rays.resize(width);
	for (size_t i = 0; i < width; i++) {
		rays[i].reserve(height);
	}
	for (size_t j = 0; j < height; j++) {
		for (size_t i = 0; i < width; i++) {
			image(i,j) = std::make_tuple(0,0,1);
		}
	}

	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_FLOAT, image);
	// std::cout << "================================================\n";
	generateRays();
	paintImage();
	// std::cout << "================================================\n";
}

void tnw::Raycast::generateRays() {
	auto dx = 2.0/width,
		 dy = 2.0/height;
	for (size_t j = 0; j < height; ++j) {
		for (size_t i = 0; i < width; ++i) {

			auto x = -2.f*(1.f/2.f - float(i)/width);
			auto y = 2.f*(1.f/2.f - float(j)/height);

			float pospixx = x+dx/2;
			float pospixy = y-dy/2;

			// std::cout << "untransf a: " << glm::to_string(glm::vec3(pospixx, pospixy, camera.near)) << " unstransf b: " << glm::to_string(glm::vec3(pospixx, pospixy, camera.far)) << std::endl;

			auto cameraMatrix = glm::mat4(1/camera.scale);
			cameraMatrix = glm::translate(glm::mat4(),-camera.pos) * cameraMatrix;
			cameraMatrix = glm::inverse(tnw::isometric(camera.aspect, -camera.near, -camera.far, camera.positive_hor, camera.positive_ver)) * cameraMatrix;

			// auto aM = glm::rotate(glm::mat4(), glm::radians(-45.0f), glm::vec3(0.0,1.0,0.0));
			// auto bM = glm::rotate(glm::mat4(), -std::asin(std::tan(glm::radians(30.0f))), glm::vec3(1.0,0.0,0.0));
			// cameraMatrix = aM*bM*cameraMatrix;

			glm::vec4 a = cameraMatrix * glm::vec4(pospixx, pospixy, camera.near, 1.0f);
			glm::vec4 b = cameraMatrix * glm::vec4(pospixx, pospixy, camera.far, 1.0f);

			// std::cout << "i: " << i << " j: " << j << "\n";
			// std::cout << "a: " << glm::to_string(a) << "b: " << glm::to_string(b) << std::endl;

			// tnw::Ray r = tnw::Ray(a,b);
			// rays[i][j] = r;
			rays[i].push_back(tnw::Ray(glm::vec3(a),glm::vec3(b)));
		}
	}
}

void tnw::Raycast::paintImage() {

	for (size_t j = 0; j < height; ++j) {
		for (size_t i = 0; i < width; ++i) {
			PaintColor paintColor = {0.0, 0.0, 0.0};
			float maxDist = glm::length(rays[i][j].dir);
			// std::cout << "maxDist: " << maxDist << "\n";
			std::tuple<tnw::Color, float> minInter = std::make_tuple(tnw::Color::white, maxDist);
			// printf("%zu,%zu\n",i,j);
			for (auto&& model : models) {
				if (!model->visible) continue;

				tnw::IntersectionList ilist = model->intersect_ray(rays[i][j]);
				assert(ilist.size() > 0);

				switch (std::get<0>(ilist[0])) {
					case tnw::Color::black:
					case tnw::Color::gray:
						if (std::get<1>(minInter) > std::get<1>(ilist[0])) {
							minInter = ilist[0];
							paintColor = model->getColor();
						}
						break;
					case tnw::Color::white:
						if (std::get<1>(ilist[0]) < maxDist && std::get<1>(minInter) > std::get<1>(ilist[0])) {
							minInter = ilist[0];
							paintColor = model->getColor();
						}
				}
			}
			image(i,j) = std::make_tuple(paintColor[0], paintColor[1], paintColor[2]);
		}
	}

	glTexSubImage2D(GL_TEXTURE_2D, 0 ,0, 0, width, height, GL_RGB, GL_FLOAT, image);
}
<|endoftext|>


<commit_before><commit_msg>utf-16, not utf-8<commit_after><|endoftext|>