<commit_before>/*  Copyright (C) 2009 Imperial College London and others.

    Please see the AUTHORS file in the main source directory for a full list
    of copyright holders.

    Prof. C Pain
    Applied Modelling and Computation Group
    Department of Earth Science and Engineering
    Imperial College London

    amcgsoftware@imperial.ac.uk

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation,
    version 2.1 of the License.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
    USA
*/
#include "confdefs.h"
#include "NEMOReader.h"

using namespace std;

NEMOReader NEMOReader_v2_global;

extern int projections(int nPoints, double *x, double *y, double *z, string current_coord, string output_coord);

extern "C" {
#define rotate_ll2cart_fc F77_FUNC(rotate_ll2cart, ROTATE_LL2CART)
  extern void rotate_ll2cart_fc(double *longitude, double *latitude, double *u, double *v,
                     const double *r3u, const double *r3v, const double *r3w);

#define get_nemo_variables_fc F77_FUNC(get_nemo_variables, GET_NEMO_VARIABLES)
    void get_nemo_variables_fc(double *time, const double *X, const double *Y, const double *Z, const double *DEPTH,
                     double *Te, double *Sa, double *U, double *V, double *W, double *SSH, const int *NNodes);
}

void get_nemo_variables_fc(double *time, const double *X, const double *Y, const double *Z, const double *DEPTH,
                     double *Te, double *Sa, double *U, double *V, double *W, double *SSH, const int *n){

    const int nFields = 5; // number of fields being read in, currently temperature, salinity, U, V and sea surface height
    const int NNodes = *n;

    double *salinity = new double[NNodes];
    double *seatemp = new double[NNodes];
    double *uvel = new double[NNodes];
    double *vvel = new double[NNodes];
    double *wvel = new double[NNodes];
    double *seash = new double[NNodes];

    NEMOReader_v2_global.SetTimeSeconds(*time);

    // convert from Cart to long-lat
    double *x = new double[NNodes];
    double *y = new double[NNodes];
    double *z = new double[NNodes];
    double *depth = new double[NNodes];
    for (int i = 0; i < NNodes; i++) {
        x[i] = X[i];
        y[i] = Y[i];
        z[i] = Z[i];
//         depth[i]=6371010.0-sqrt(X[i]*X[i]+Y[i]*Y[i]+Z[i]*Z[i]);
        depth[i] = DEPTH[i];
    }

    int ret = projections(NNodes, x, y, z, "cart", "spherical");
    if (ret != 0) {
        cerr<<"Error converting coord system"<<endl;
    }

    /*
     * The "values" array below contains the scalar
     * values from the NEMOdata netCDF file. The table
     * below shows which NEMO parameter is at which
     * array index, along with a physical meaning
     *
    NEMO field  | Index  | Physical meaning   |     units       |
    ------------+-------+---------------------+-----------------+
    temperature |   0   | Sea Temperature     | degrees celcius |
    salinity    |   1   | Salinity            | PPT             |
    u           |   2   | azimuthal velocity  | ms^-1           |
    v           |   3   | meridional velocity | ms^-1           |
    ssh         |   4   | Sea surface height  | m               |
    */

    // loop over nodes
    for (int i=0; i<NNodes; i++) {

        double latitude = y[i];
        double longitude = x[i];
        double depth_p = depth[i];

        // get values from the netcdf file
        double values[nFields];

        NEMOReader_v2_global.GetScalars(longitude, latitude, depth_p, values);

        // values contains the values above in the order we registered them
        // See above
        seatemp[i] = values[0];
        salinity[i] = values[1];
        seash[i] = values[4];

        // rotate wind to cartesian grid
        double u_rot = values[2];
        double v_rot = values[3];

        // This currently assumes that you are running on the sphere. If NEMO data is going to be used for
        // planar cases an if if(have_option("/geometry/spherical_earth")) will need to be passed or
        // added.
        rotate_ll2cart_fc(&longitude, &latitude, &u_rot, &v_rot, &uvel[i], &vvel[i], &wvel[i]);

    }

    delete [] x;
    delete [] y;
    delete [] z;
    delete [] depth;

    for (int i=0; i<NNodes; i++) {

      Te[i]=seatemp[i];
      Sa[i]=salinity[i];
      U[i]=uvel[i];
      V[i]=vvel[i];
      W[i]=wvel[i];
      SSH[i]=seash[i];

    }

  // clean up
  delete [] salinity;
  delete [] seatemp;
  delete [] uvel;
  delete [] vvel;
  delete [] wvel;
  delete [] seash;

}

#ifdef NEMODATALOAD_UNITTEST
#include <vector>
#include <vtk.h>

void vtk_add_scalar(vector<double> &scalar, const char *scalar_name, vtkUnstructuredGrid *ug){
  int NNodes = ug->GetNumberOfPoints();

  vtkDoubleArray *newScalars = vtkDoubleArray::New();
  newScalars->SetName(scalar_name);
  newScalars->SetNumberOfComponents(1);
  newScalars->SetNumberOfTuples(NNodes);

  for(int i=0; i<NNodes; i++)
    newScalars->InsertValue(i, scalar[i]);

  ug->GetPointData()->AddArray(newScalars);
  ug->GetPointData()->SetActiveAttribute(scalar_name, vtkDataSetAttributes::SCALARS);

  ug->Update();
  newScalars->Delete();
}

void vtk_add_vector(vector<double> &vx, vector<double> &vy, vector<double> &vz, const char *vector_name, vtkUnstructuredGrid *ug){
  int NNodes = ug->GetNumberOfPoints();

  vtkDoubleArray *newVectors = vtkDoubleArray::New();
  newVectors->SetName(vector_name);
  newVectors->SetNumberOfComponents(3);
  newVectors->SetNumberOfTuples(NNodes);

  for(int i=0; i<NNodes; i++)
    newVectors->SetTuple3(i, vx[i], vy[i], vz[i]);;

  ug->GetPointData()->AddArray(newVectors);
  ug->GetPointData()->SetActiveAttribute(vector_name, vtkDataSetAttributes::VECTORS);

  ug->Update();
  newVectors->Delete();
}

// This is a simple test program to view the interpolation of the NEMO data onto an
// unstructured grid supplied from a vtu.
int main(int argc, char **argv){

    char *filename=argv[1];
    vtkXMLUnstructuredGridReader *reader = vtkXMLUnstructuredGridReader::New();
    reader->SetFileName(filename);
    reader->Update();

    vtkUnstructuredGrid *ug = reader->GetOutput();

    int NNodes = ug->GetNumberOfPoints();
    double time = 0.0;
    vector<double> Te(NNodes, 0.0), Sa(NNodes, 0.0), U(NNodes, 0.0), V(NNodes, 0.0),
    W(NNodes, 0.0), SSH(NNodes, 0.0), X(NNodes, 0.0), Y(NNodes, 0.0), Z(NNodes, 0.0),
    x(NNodes,0.0), y(NNodes,0.0), z(NNodes,0.0), r(NNodes,0.0), depth(NNodes,0.0);

    for (int i=0; i<NNodes; i++) {
        double r[3];
        ug->GetPoints()->GetPoint(i, r);
        X[i] = r[0];
        Y[i] = r[1];
        Z[i] = r[2];
        // these will be turned into lat/long
        x[i] = X[i];
        y[i] = Y[i];
        z[i] = Z[i];
        r[i] = sqrt(X[i]*X[i]+Y[i]*Y[i]+Z[i]*Z[i]);
    }

    double REdum=0.0;

    for (int i=0; i<NNodes; i++) {
      if(r[i]>REdum) REdum=r[i];
    }

    for (int i=0; i<NNodes; i++) {
      depth[i]=REdum-r[i];
    }

    NEMOReader NEMOReader_NEMOdata;

    // this data set is the input
//     NEMOReader_v2_global.RegisterDataFile("/data/ORCA_grid/gmt_gridding_nwa/NEMOnwa.nc");
    NEMOReader_v2_global.RegisterDataFile("/data/ORCA_grid/gmt_gridding_med/NEMOmed.nc");

    NEMOReader_v2_global.SetSimulationTimeUnits("seconds since 1987-01-05 00:00:0.0");
    NEMOReader_v2_global.AddFieldOfInterest("temperature");  //  0   | Sea temperature
    NEMOReader_v2_global.AddFieldOfInterest("salinity");     //  1   | Salinity
    NEMOReader_v2_global.AddFieldOfInterest("u");            //  2   | Azimuthal velocity
    NEMOReader_v2_global.AddFieldOfInterest("v");            //  3   | Meridional velocity
    NEMOReader_v2_global.AddFieldOfInterest("ssh");          //  4   | Sea surface height

    int ret = projections(NNodes, &x[0], &y[0], &z[0], "cart", "spherical");
    if (ret != 0) {
        cerr<<"Error converting coord system"<<endl;
    }
    int const nFields_in=4;
    double values_in[nFields_in];

    // now calucalte fluxes for the whole globe and put on the output VTU file for
    // visualisation

    // Note that latitudinal velocities are currently missing from the NEMO data
    // and is thus set to zero.

    get_nemo_variables_fc(&time, &X[0], &Y[0], &Z[0], &depth[0], &Te[0], &Sa[0], &U[0], &V[0], &W[0],
                        &SSH[0], &NNodes);

    // add to VTU file
    vtk_add_scalar(Te, "Temperature", ug);
    vtk_add_scalar(Sa, "Salinity", ug);
    vtk_add_scalar(U, "U", ug);
    vtk_add_scalar(V, "V", ug);
    vtk_add_scalar(W, "W", ug);
    vtk_add_vector(U, V, W, "Velocity", ug);
    vtk_add_scalar(SSH, "Sea surface height", ug);

    // now print values at a single node
    int n = 1;
    // A point in the North West Approaches
    double londum,latdum,pidum,location_depth;
    pidum=acos(-1.0);
//     londum=(-8.0)*pidum/180.0;
//     latdum=(60.0)*pidum/180.0;
    londum=(-5.0)*pidum/180.0;
    latdum=(40.0)*pidum/180.0;
    location_depth=50.0
    X[0] = (REdum-location_depth)*cos(latdum)*cos(londum);
    Y[0] = (REdum-location_depth)*cos(latdum)*sin(londum);
    Z[0] = (REdum-location_depth)*sin(latdum);
    y[0] = Y[0] , x[0] = X[0] , z[0] = Z[0];
    ret = projections(n, &x[0], &y[0], &z[0], "cart", "spherical");
    if (ret != 0) {
        cerr<<"Error converting coord system"<<endl;
    }

    NEMOReader_v2_global.GetScalars(x[0], y[0], location_depth, values_in);

    Te[0] = values_in[0];
    Sa[0] = values_in[1];
    SSH[0] = values_in[4];

    // rotate wind to cartesian grid
    double u_sphere = values_in[2];
    double v_sphere = values_in[3];

    // This currently assumes that you are running on the sphere. If NEMO data is going to be used for
    // planar cases an if if(have_option("/geometry/spherical_earth")) will need to be passed or
    // added.
    rotate_ll2cart_fc(&londum, &latdum, &u_sphere, &v_sphere, &U[0], &V[0], &W[0]);

    cout <<endl<<"Values for "<<x[0]<<","<<y[0]<<endl<<endl;

    cout <<"    Variable         |     NEMO value     |"<<endl;
    cout <<"---------------------+--------------------|"<<endl;
    cout <<"    Temperature      |    "<<Te[0] << endl;
    cout <<"     Salinity        |    "<<Sa[0] << endl;
    cout <<"        U            |    "<<U[0] << endl;
    cout <<"        V            |    "<<V[0] << endl;
    cout <<"        W            |    "<<W[0] << endl;
    cout <<"       SSH           |    "<<SSH[0] << endl;

  vtkXMLUnstructuredGridWriter *writer = vtkXMLUnstructuredGridWriter::New();
//   writer->SetFileName("NEMOvalues_nwa.vtu");
  writer->SetFileName("NEMOvalues_med.vtu");
  writer->SetInput(ug);
  writer->Write();
}
#endif<commit_msg>Removed another hard coded instance of Earth radius. This should not matter, as the line corrected was commented out.<commit_after>/*  Copyright (C) 2009 Imperial College London and others.

    Please see the AUTHORS file in the main source directory for a full list
    of copyright holders.

    Prof. C Pain
    Applied Modelling and Computation Group
    Department of Earth Science and Engineering
    Imperial College London

    amcgsoftware@imperial.ac.uk

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation,
    version 2.1 of the License.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
    USA
*/
#include "confdefs.h"
#include "NEMOReader.h"
#include "global_parameters.h"

using namespace std;

NEMOReader NEMOReader_v2_global;

extern int projections(int nPoints, double *x, double *y, double *z, string current_coord, string output_coord);

extern "C" {
#define rotate_ll2cart_fc F77_FUNC(rotate_ll2cart, ROTATE_LL2CART)
  extern void rotate_ll2cart_fc(double *longitude, double *latitude, double *u, double *v,
                     const double *r3u, const double *r3v, const double *r3w);

#define get_nemo_variables_fc F77_FUNC(get_nemo_variables, GET_NEMO_VARIABLES)
    void get_nemo_variables_fc(double *time, const double *X, const double *Y, const double *Z, const double *DEPTH,
                     double *Te, double *Sa, double *U, double *V, double *W, double *SSH, const int *NNodes);
}

void get_nemo_variables_fc(double *time, const double *X, const double *Y, const double *Z, const double *DEPTH,
                     double *Te, double *Sa, double *U, double *V, double *W, double *SSH, const int *n){

    const int nFields = 5; // number of fields being read in, currently temperature, salinity, U, V and sea surface height
    const int NNodes = *n;

    double *salinity = new double[NNodes];
    double *seatemp = new double[NNodes];
    double *uvel = new double[NNodes];
    double *vvel = new double[NNodes];
    double *wvel = new double[NNodes];
    double *seash = new double[NNodes];

    NEMOReader_v2_global.SetTimeSeconds(*time);

    // convert from Cart to long-lat
    double *x = new double[NNodes];
    double *y = new double[NNodes];
    double *z = new double[NNodes];
    double *depth = new double[NNodes];
    for (int i = 0; i < NNodes; i++) {
        x[i] = X[i];
        y[i] = Y[i];
        z[i] = Z[i];
//         depth[i] = get_surface_radius() - sqrt(X[i]*X[i]+Y[i]*Y[i]+Z[i]*Z[i]);
        depth[i] = DEPTH[i];
    }

    int ret = projections(NNodes, x, y, z, "cart", "spherical");
    if (ret != 0) {
        cerr<<"Error converting coord system"<<endl;
    }

    /*
     * The "values" array below contains the scalar
     * values from the NEMOdata netCDF file. The table
     * below shows which NEMO parameter is at which
     * array index, along with a physical meaning
     *
    NEMO field  | Index  | Physical meaning   |     units       |
    ------------+-------+---------------------+-----------------+
    temperature |   0   | Sea Temperature     | degrees celcius |
    salinity    |   1   | Salinity            | PPT             |
    u           |   2   | azimuthal velocity  | ms^-1           |
    v           |   3   | meridional velocity | ms^-1           |
    ssh         |   4   | Sea surface height  | m               |
    */

    // loop over nodes
    for (int i=0; i<NNodes; i++) {

        double latitude = y[i];
        double longitude = x[i];
        double depth_p = depth[i];

        // get values from the netcdf file
        double values[nFields];

        NEMOReader_v2_global.GetScalars(longitude, latitude, depth_p, values);

        // values contains the values above in the order we registered them
        // See above
        seatemp[i] = values[0];
        salinity[i] = values[1];
        seash[i] = values[4];

        // rotate wind to cartesian grid
        double u_rot = values[2];
        double v_rot = values[3];

        // This currently assumes that you are running on the sphere. If NEMO data is going to be used for
        // planar cases an if if(have_option("/geometry/spherical_earth")) will need to be passed or
        // added.
        rotate_ll2cart_fc(&longitude, &latitude, &u_rot, &v_rot, &uvel[i], &vvel[i], &wvel[i]);

    }

    delete [] x;
    delete [] y;
    delete [] z;
    delete [] depth;

    for (int i=0; i<NNodes; i++) {

      Te[i]=seatemp[i];
      Sa[i]=salinity[i];
      U[i]=uvel[i];
      V[i]=vvel[i];
      W[i]=wvel[i];
      SSH[i]=seash[i];

    }

  // clean up
  delete [] salinity;
  delete [] seatemp;
  delete [] uvel;
  delete [] vvel;
  delete [] wvel;
  delete [] seash;

}

#ifdef NEMODATALOAD_UNITTEST
#include <vector>
#include <vtk.h>

void vtk_add_scalar(vector<double> &scalar, const char *scalar_name, vtkUnstructuredGrid *ug){
  int NNodes = ug->GetNumberOfPoints();

  vtkDoubleArray *newScalars = vtkDoubleArray::New();
  newScalars->SetName(scalar_name);
  newScalars->SetNumberOfComponents(1);
  newScalars->SetNumberOfTuples(NNodes);

  for(int i=0; i<NNodes; i++)
    newScalars->InsertValue(i, scalar[i]);

  ug->GetPointData()->AddArray(newScalars);
  ug->GetPointData()->SetActiveAttribute(scalar_name, vtkDataSetAttributes::SCALARS);

  ug->Update();
  newScalars->Delete();
}

void vtk_add_vector(vector<double> &vx, vector<double> &vy, vector<double> &vz, const char *vector_name, vtkUnstructuredGrid *ug){
  int NNodes = ug->GetNumberOfPoints();

  vtkDoubleArray *newVectors = vtkDoubleArray::New();
  newVectors->SetName(vector_name);
  newVectors->SetNumberOfComponents(3);
  newVectors->SetNumberOfTuples(NNodes);

  for(int i=0; i<NNodes; i++)
    newVectors->SetTuple3(i, vx[i], vy[i], vz[i]);;

  ug->GetPointData()->AddArray(newVectors);
  ug->GetPointData()->SetActiveAttribute(vector_name, vtkDataSetAttributes::VECTORS);

  ug->Update();
  newVectors->Delete();
}

// This is a simple test program to view the interpolation of the NEMO data onto an
// unstructured grid supplied from a vtu.
int main(int argc, char **argv){

    char *filename=argv[1];
    vtkXMLUnstructuredGridReader *reader = vtkXMLUnstructuredGridReader::New();
    reader->SetFileName(filename);
    reader->Update();

    vtkUnstructuredGrid *ug = reader->GetOutput();

    int NNodes = ug->GetNumberOfPoints();
    double time = 0.0;
    vector<double> Te(NNodes, 0.0), Sa(NNodes, 0.0), U(NNodes, 0.0), V(NNodes, 0.0),
    W(NNodes, 0.0), SSH(NNodes, 0.0), X(NNodes, 0.0), Y(NNodes, 0.0), Z(NNodes, 0.0),
    x(NNodes,0.0), y(NNodes,0.0), z(NNodes,0.0), r(NNodes,0.0), depth(NNodes,0.0);

    for (int i=0; i<NNodes; i++) {
        double r[3];
        ug->GetPoints()->GetPoint(i, r);
        X[i] = r[0];
        Y[i] = r[1];
        Z[i] = r[2];
        // these will be turned into lat/long
        x[i] = X[i];
        y[i] = Y[i];
        z[i] = Z[i];
        r[i] = sqrt(X[i]*X[i]+Y[i]*Y[i]+Z[i]*Z[i]);
    }

    double REdum=0.0;

    for (int i=0; i<NNodes; i++) {
      if(r[i]>REdum) REdum=r[i];
    }

    for (int i=0; i<NNodes; i++) {
      depth[i]=REdum-r[i];
    }

    NEMOReader NEMOReader_NEMOdata;

    // this data set is the input
//     NEMOReader_v2_global.RegisterDataFile("/data/ORCA_grid/gmt_gridding_nwa/NEMOnwa.nc");
    NEMOReader_v2_global.RegisterDataFile("/data/ORCA_grid/gmt_gridding_med/NEMOmed.nc");

    NEMOReader_v2_global.SetSimulationTimeUnits("seconds since 1987-01-05 00:00:0.0");
    NEMOReader_v2_global.AddFieldOfInterest("temperature");  //  0   | Sea temperature
    NEMOReader_v2_global.AddFieldOfInterest("salinity");     //  1   | Salinity
    NEMOReader_v2_global.AddFieldOfInterest("u");            //  2   | Azimuthal velocity
    NEMOReader_v2_global.AddFieldOfInterest("v");            //  3   | Meridional velocity
    NEMOReader_v2_global.AddFieldOfInterest("ssh");          //  4   | Sea surface height

    int ret = projections(NNodes, &x[0], &y[0], &z[0], "cart", "spherical");
    if (ret != 0) {
        cerr<<"Error converting coord system"<<endl;
    }
    int const nFields_in=4;
    double values_in[nFields_in];

    // now calucalte fluxes for the whole globe and put on the output VTU file for
    // visualisation

    // Note that latitudinal velocities are currently missing from the NEMO data
    // and is thus set to zero.

    get_nemo_variables_fc(&time, &X[0], &Y[0], &Z[0], &depth[0], &Te[0], &Sa[0], &U[0], &V[0], &W[0],
                        &SSH[0], &NNodes);

    // add to VTU file
    vtk_add_scalar(Te, "Temperature", ug);
    vtk_add_scalar(Sa, "Salinity", ug);
    vtk_add_scalar(U, "U", ug);
    vtk_add_scalar(V, "V", ug);
    vtk_add_scalar(W, "W", ug);
    vtk_add_vector(U, V, W, "Velocity", ug);
    vtk_add_scalar(SSH, "Sea surface height", ug);

    // now print values at a single node
    int n = 1;
    // A point in the North West Approaches
    double londum,latdum,pidum,location_depth;
    pidum=acos(-1.0);
//     londum=(-8.0)*pidum/180.0;
//     latdum=(60.0)*pidum/180.0;
    londum=(-5.0)*pidum/180.0;
    latdum=(40.0)*pidum/180.0;
    location_depth=50.0
    X[0] = (REdum-location_depth)*cos(latdum)*cos(londum);
    Y[0] = (REdum-location_depth)*cos(latdum)*sin(londum);
    Z[0] = (REdum-location_depth)*sin(latdum);
    y[0] = Y[0] , x[0] = X[0] , z[0] = Z[0];
    ret = projections(n, &x[0], &y[0], &z[0], "cart", "spherical");
    if (ret != 0) {
        cerr<<"Error converting coord system"<<endl;
    }

    NEMOReader_v2_global.GetScalars(x[0], y[0], location_depth, values_in);

    Te[0] = values_in[0];
    Sa[0] = values_in[1];
    SSH[0] = values_in[4];

    // rotate wind to cartesian grid
    double u_sphere = values_in[2];
    double v_sphere = values_in[3];

    // This currently assumes that you are running on the sphere. If NEMO data is going to be used for
    // planar cases an if if(have_option("/geometry/spherical_earth")) will need to be passed or
    // added.
    rotate_ll2cart_fc(&londum, &latdum, &u_sphere, &v_sphere, &U[0], &V[0], &W[0]);

    cout <<endl<<"Values for "<<x[0]<<","<<y[0]<<endl<<endl;

    cout <<"    Variable         |     NEMO value     |"<<endl;
    cout <<"---------------------+--------------------|"<<endl;
    cout <<"    Temperature      |    "<<Te[0] << endl;
    cout <<"     Salinity        |    "<<Sa[0] << endl;
    cout <<"        U            |    "<<U[0] << endl;
    cout <<"        V            |    "<<V[0] << endl;
    cout <<"        W            |    "<<W[0] << endl;
    cout <<"       SSH           |    "<<SSH[0] << endl;

  vtkXMLUnstructuredGridWriter *writer = vtkXMLUnstructuredGridWriter::New();
//   writer->SetFileName("NEMOvalues_nwa.vtu");
  writer->SetFileName("NEMOvalues_med.vtu");
  writer->SetInput(ug);
  writer->Write();
}
#endif
<|endoftext|>


<commit_before>/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

#ifdef MSVC
#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
#endif

// select correct drawing functions

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCappedCylinder dsDrawCappedCylinderD
#endif

// some constants

#define NUM 20			// max number of objects
#define DENSITY (5.0)		// density of all objects
#define GPB 3			// maximum number of geometries per body

// dynamics and collision objects

struct MyObject {
  dBodyID body;			// the body
  dGeomID geom[GPB];		// geometries representing this body
};

static int num=0;		// number of objects in simulation
static int nextobj=0;		// next object to recycle if num==NUM
static dWorldID world;
static dSpaceID space;
static MyObject obj[NUM];
static dJointGroupID contactgroup;
static int selected = -1;	// selected object
static int show_aabb = 0;	// show geom AABBs?

// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i;
  // if (o1->body && o2->body) return;

  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnected (b1,b2)) return;

  dContact contact[3];			// up to 3 contacts per box
  for (i=0; i<3; i++) {
    contact[i].surface.mode = dContactBounce; //dContactMu2;
    contact[i].surface.mu = dInfinity;
    contact[i].surface.mu2 = 0;
    contact[i].surface.bounce = 0.5;
    contact[i].surface.bounce_vel = 0.1;
  }
  if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) {
    // dMatrix3 RI;
    // dRSetIdentity (RI);
    // const dReal ss[3] = {0.02,0.02,0.02};
    for (i=0; i<numc; i++) {
      dJointID c = dJointCreateContact (world,contactgroup,contact+i);
      dJointAttach (c,b1,b2);
      // dsDrawBox (contact[i].geom.pos,RI,ss);
    }
  }
}

// start simulation - set viewpoint

static void start()
{
  static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
  static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
  dsSetViewpoint (xyz,hpr);
  printf ("To drop another object, press:\n");
  printf ("   b for box.\n");
  printf ("   s for sphere.\n");
  printf ("   c for cylinder.\n");
  printf ("   x for a composite object.\n");
  printf ("To select an object, press space.\n");
  printf ("To disable the selected object, press d.\n");
  printf ("To enable the selected object, press e.\n");
  printf ("To toggle showing the geom AABBs, press a.\n");
}

char locase (char c)
{
  if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
  else return c;
}

// called when a key pressed

static void command (int cmd)
{
  int i,j,k;
  dReal sides[3];
  dMass m;

  cmd = locase (cmd);
  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
      /* || cmd == 'l' */) {
    if (num < NUM) {
      i = num;
      num++;
    }
    else {
      i = nextobj;
      nextobj++;
      if (nextobj >= num) nextobj = 0;

      // destroy the body and geoms for slot i
      dBodyDestroy (obj[i].body);
      for (k=0; k < GPB; k++) {
	if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
      }
      memset (&obj[i],0,sizeof(obj[i]));
    }

    obj[i].body = dBodyCreate (world);
    for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;

    dBodySetPosition (obj[i].body,
		      dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
    dMatrix3 R;
    dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
    dBodySetRotation (obj[i].body,R);
    dBodySetData (obj[i].body,(void*) i);

    if (cmd == 'b') {
      dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
      obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
    }
    else if (cmd == 'c') {
      sides[0] *= 0.5;
      dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
    }
/*
    // cylinder option not yet implemented
    else if (cmd == 'l') {
      sides[1] *= 0.5;
      dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
    }
*/
    else if (cmd == 's') {
      sides[0] *= 0.5;
      dMassSetSphere (&m,DENSITY,sides[0]);
      obj[i].geom[0] = dCreateSphere (space,sides[0]);
    }
    else if (cmd == 'x') {
      dGeomID g2[GPB];		// encapsulated geometries
      dReal dpos[GPB][3];	// delta-positions for encapsulated geometries

      // start accumulating masses for the encapsulated geometries
      dMass m2;
      dMassSetZero (&m);

      // set random delta positions
      for (j=0; j<GPB; j++) {
	for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
      }

      for (k=0; k<3; k++) {
	obj[i].geom[k] = dCreateGeomTransform (space);
	dGeomTransformSetCleanup (obj[i].geom[k],1);
	if (k==0) {
	  dReal radius = dRandReal()*0.25+0.05;
	  g2[k] = dCreateSphere (0,radius);
	  dMassSetSphere (&m2,DENSITY,radius);
	}
	else if (k==1) {
	  g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
	  dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
	}
	else {
	  dReal radius = dRandReal()*0.1+0.05;
	  dReal length = dRandReal()*1.0+0.1;
	  g2[k] = dCreateCCylinder (0,radius,length);
	  dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
	}
	dGeomTransformSetGeom (obj[i].geom[k],g2[k]);

	// set the transformation (adjust the mass too)
	dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
	dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
	dMatrix3 Rtx;
	dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			    dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
	dGeomSetRotation (g2[k],Rtx);
	dMassRotate (&m2,Rtx);

	// add to the total mass
	dMassAdd (&m,&m2);
      }

      // move all encapsulated objects so that the center of mass is (0,0,0)
      for (k=0; k<2; k++) {
	dGeomSetPosition (g2[k],
			  dpos[k][0]-m.c[0],
			  dpos[k][1]-m.c[1],
			  dpos[k][2]-m.c[2]);
      }
      dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
    }

    for (k=0; k < GPB; k++) {
      if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
    }

    dBodySetMass (obj[i].body,&m);
  }

  if (cmd == ' ') {
    selected++;
    if (selected >= num) selected = 0;
    if (selected < 0) selected = 0;
  }
  else if (cmd == 'd' && selected >= 0 && selected < num) {
    dBodyDisable (obj[selected].body);
  }
  else if (cmd == 'e' && selected >= 0 && selected < num) {
    dBodyEnable (obj[selected].body);
  }
  else if (cmd == 'a') {
    show_aabb ^= 1;
  }
}

// draw a geom

void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
  if (!g) return;
  if (!pos) pos = dGeomGetPosition (g);
  if (!R) R = dGeomGetRotation (g);

  int type = dGeomGetClass (g);
  if (type == dBoxClass) {
    dVector3 sides;
    dGeomBoxGetLengths (g,sides);
    dsDrawBox (pos,R,sides);
  }
  else if (type == dSphereClass) {
    dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
  }
  else if (type == dCCylinderClass) {
    dReal radius,length;
    dGeomCCylinderGetParams (g,&radius,&length);
    dsDrawCappedCylinder (pos,R,length,radius);
  }
/*
  // cylinder option not yet implemented
  else if (type == dCylinderClass) {
    dReal radius,length;
    dGeomCylinderGetParams (g,&radius,&length);
    dsDrawCylinder (pos,R,length,radius);
  }
*/
  else if (type == dGeomTransformClass) {
    dGeomID g2 = dGeomTransformGetGeom (g);
    const dReal *pos2 = dGeomGetPosition (g2);
    const dReal *R2 = dGeomGetRotation (g2);
    dVector3 actual_pos;
    dMatrix3 actual_R;
    dMULTIPLY0_331 (actual_pos,R,pos2);
    actual_pos[0] += pos[0];
    actual_pos[1] += pos[1];
    actual_pos[2] += pos[2];
    dMULTIPLY0_333 (actual_R,R,R2);
    drawGeom (g2,actual_pos,actual_R,0);
  }

  if (show_aabb) {
    // draw the bounding box for this geom
    dReal aabb[6];
    dGeomGetAABB (g,aabb);
    dVector3 bbpos;
    for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
    dVector3 bbsides;
    for (int i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
    dMatrix3 RI;
    dRSetIdentity (RI);
    dsSetColorAlpha (1,0,0,0.5);
    dsDrawBox (bbpos,RI,bbsides);
  }
}

// simulation loop

static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldStep (world,0.05);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}

int main (int argc, char **argv)
{
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = "../../drawstuff/textures";

  // create world

  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-0.5);
  dWorldSetCFM (world,1e-5);
  dCreatePlane (space,0,0,1,0);
  memset (obj,0,sizeof(obj));

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);

  return 0;
}
<commit_msg>added non-random dropping mode to be able to stack boxes. added contact point showing mode.<commit_after>/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

#ifdef MSVC
#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
#endif

// select correct drawing functions

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCappedCylinder dsDrawCappedCylinderD
#endif

// some constants

#define NUM 20			// max number of objects
#define DENSITY (5.0)		// density of all objects
#define GPB 3			// maximum number of geometries per body

// dynamics and collision objects

struct MyObject {
  dBodyID body;			// the body
  dGeomID geom[GPB];		// geometries representing this body
};

static int num=0;		// number of objects in simulation
static int nextobj=0;		// next object to recycle if num==NUM
static dWorldID world;
static dSpaceID space;
static MyObject obj[NUM];
static dJointGroupID contactgroup;
static int selected = -1;	// selected object
static int show_aabb = 0;	// show geom AABBs?
static int show_contacts = 0;	// show contact points?
static int random_pos = 1;	// drop objects from random position?

// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i;
  // if (o1->body && o2->body) return;

  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnected (b1,b2)) return;

  dContact contact[4];			// up to 4 contacts per box-box
  for (i=0; i<4; i++) {
    contact[i].surface.mode = dContactBounce | dContactSoftCFM;
    contact[i].surface.mu = dInfinity;
    contact[i].surface.mu2 = 0;
    contact[i].surface.bounce = 0.1;
    contact[i].surface.bounce_vel = 0.1;
    contact[i].surface.soft_cfm = 0.01;
  }
  if (int numc = dCollide (o1,o2,4,&contact[0].geom,sizeof(dContact))) {
    dMatrix3 RI;
    dRSetIdentity (RI);
    const dReal ss[3] = {0.02,0.02,0.02};
    for (i=0; i<numc; i++) {
      dJointID c = dJointCreateContact (world,contactgroup,contact+i);
      dJointAttach (c,b1,b2);
      if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
    }
  }
}

// start simulation - set viewpoint

static void start()
{
  static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
  static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
  dsSetViewpoint (xyz,hpr);
  printf ("To drop another object, press:\n");
  printf ("   b for box.\n");
  printf ("   s for sphere.\n");
  printf ("   c for cylinder.\n");
  printf ("   x for a composite object.\n");
  printf ("To select an object, press space.\n");
  printf ("To disable the selected object, press d.\n");
  printf ("To enable the selected object, press e.\n");
  printf ("To toggle showing the geom AABBs, press a.\n");
  printf ("To toggle showing the contact points, press t.\n");
  printf ("To toggle dropping from random position/orientation, press r.\n");
}

char locase (char c)
{
  if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
  else return c;
}

// called when a key pressed

static void command (int cmd)
{
  int i,j,k;
  dReal sides[3];
  dMass m;

  cmd = locase (cmd);
  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
      /* || cmd == 'l' */) {
    if (num < NUM) {
      i = num;
      num++;
    }
    else {
      i = nextobj;
      nextobj++;
      if (nextobj >= num) nextobj = 0;

      // destroy the body and geoms for slot i
      dBodyDestroy (obj[i].body);
      for (k=0; k < GPB; k++) {
	if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
      }
      memset (&obj[i],0,sizeof(obj[i]));
    }

    obj[i].body = dBodyCreate (world);
    for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;

    dMatrix3 R;
    if (random_pos) {
      dBodySetPosition (obj[i].body,
			dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
      dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			  dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
    }
    else {
      dReal maxheight = 0;
      for (k=0; k<num; k++) {
	const dReal *pos = dBodyGetPosition (obj[k].body);
	if (pos[2] > maxheight) maxheight = pos[2];
      }
      dBodySetPosition (obj[i].body, 0,0,maxheight+1);
      dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
    }
    dBodySetRotation (obj[i].body,R);
    dBodySetData (obj[i].body,(void*) i);

    if (cmd == 'b') {
      dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
      obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
    }
    else if (cmd == 'c') {
      sides[0] *= 0.5;
      dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
    }
/*
    // cylinder option not yet implemented
    else if (cmd == 'l') {
      sides[1] *= 0.5;
      dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
    }
*/
    else if (cmd == 's') {
      sides[0] *= 0.5;
      dMassSetSphere (&m,DENSITY,sides[0]);
      obj[i].geom[0] = dCreateSphere (space,sides[0]);
    }
    else if (cmd == 'x') {
      dGeomID g2[GPB];		// encapsulated geometries
      dReal dpos[GPB][3];	// delta-positions for encapsulated geometries

      // start accumulating masses for the encapsulated geometries
      dMass m2;
      dMassSetZero (&m);

      // set random delta positions
      for (j=0; j<GPB; j++) {
	for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
      }

      for (k=0; k<3; k++) {
	obj[i].geom[k] = dCreateGeomTransform (space);
	dGeomTransformSetCleanup (obj[i].geom[k],1);
	if (k==0) {
	  dReal radius = dRandReal()*0.25+0.05;
	  g2[k] = dCreateSphere (0,radius);
	  dMassSetSphere (&m2,DENSITY,radius);
	}
	else if (k==1) {
	  g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
	  dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
	}
	else {
	  dReal radius = dRandReal()*0.1+0.05;
	  dReal length = dRandReal()*1.0+0.1;
	  g2[k] = dCreateCCylinder (0,radius,length);
	  dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
	}
	dGeomTransformSetGeom (obj[i].geom[k],g2[k]);

	// set the transformation (adjust the mass too)
	dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
	dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
	dMatrix3 Rtx;
	dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			    dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
	dGeomSetRotation (g2[k],Rtx);
	dMassRotate (&m2,Rtx);

	// add to the total mass
	dMassAdd (&m,&m2);
      }

      // move all encapsulated objects so that the center of mass is (0,0,0)
      for (k=0; k<2; k++) {
	dGeomSetPosition (g2[k],
			  dpos[k][0]-m.c[0],
			  dpos[k][1]-m.c[1],
			  dpos[k][2]-m.c[2]);
      }
      dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
    }

    for (k=0; k < GPB; k++) {
      if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
    }

    dBodySetMass (obj[i].body,&m);
  }

  if (cmd == ' ') {
    selected++;
    if (selected >= num) selected = 0;
    if (selected < 0) selected = 0;
  }
  else if (cmd == 'd' && selected >= 0 && selected < num) {
    dBodyDisable (obj[selected].body);
  }
  else if (cmd == 'e' && selected >= 0 && selected < num) {
    dBodyEnable (obj[selected].body);
  }
  else if (cmd == 'a') {
    show_aabb ^= 1;
  }
  else if (cmd == 't') {
    show_contacts ^= 1;
  }
  else if (cmd == 'r') {
    random_pos ^= 1;
  }
}

// draw a geom

void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
  if (!g) return;
  if (!pos) pos = dGeomGetPosition (g);
  if (!R) R = dGeomGetRotation (g);

  int type = dGeomGetClass (g);
  if (type == dBoxClass) {
    dVector3 sides;
    dGeomBoxGetLengths (g,sides);
    dsDrawBox (pos,R,sides);
  }
  else if (type == dSphereClass) {
    dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
  }
  else if (type == dCCylinderClass) {
    dReal radius,length;
    dGeomCCylinderGetParams (g,&radius,&length);
    dsDrawCappedCylinder (pos,R,length,radius);
  }
/*
  // cylinder option not yet implemented
  else if (type == dCylinderClass) {
    dReal radius,length;
    dGeomCylinderGetParams (g,&radius,&length);
    dsDrawCylinder (pos,R,length,radius);
  }
*/
  else if (type == dGeomTransformClass) {
    dGeomID g2 = dGeomTransformGetGeom (g);
    const dReal *pos2 = dGeomGetPosition (g2);
    const dReal *R2 = dGeomGetRotation (g2);
    dVector3 actual_pos;
    dMatrix3 actual_R;
    dMULTIPLY0_331 (actual_pos,R,pos2);
    actual_pos[0] += pos[0];
    actual_pos[1] += pos[1];
    actual_pos[2] += pos[2];
    dMULTIPLY0_333 (actual_R,R,R2);
    drawGeom (g2,actual_pos,actual_R,0);
  }

  if (show_aabb) {
    // draw the bounding box for this geom
    dReal aabb[6];
    dGeomGetAABB (g,aabb);
    dVector3 bbpos;
    for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
    dVector3 bbsides;
    for (int i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
    dMatrix3 RI;
    dRSetIdentity (RI);
    dsSetColorAlpha (1,0,0,0.5);
    dsDrawBox (bbpos,RI,bbsides);
  }
}

// simulation loop

static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldStep (world,0.05);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}

int main (int argc, char **argv)
{
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = "../../drawstuff/textures";

  // create world

  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-0.5);
  dWorldSetCFM (world,1e-5);
  dCreatePlane (space,0,0,1,0);
  memset (obj,0,sizeof(obj));

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);

  return 0;
}
<|endoftext|>


<commit_before>/*
 * TypeUtils.cc
 *
 * Copyright (C) 2015 Linas Vepstas
 *
 * Author: Linas Vepstas <linasvepstas@gmail.com>  December 2015
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License v3 as
 * published by the Free Software Foundation and including the
 * exceptions at http://opencog.org/wiki/Licenses
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public
 * License along with this program; if not, write to:
 * Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <opencog/atoms/base/Link.h>
#include <opencog/atoms/base/ClassServer.h>

#include <opencog/atoms/TypeNode.h>
#include <opencog/atoms/core/DefineLink.h>
#include <opencog/atoms/core/VariableList.h>

#include "FindUtils.h"

#include "TypeUtils.h"

namespace opencog {

/* ================================================================= */
/**
 * Type checker.  Returns true if `val` is of type `deep`.
 */
bool value_is_type(const Handle& spec, const Handle& val)
{
	Handle deep(spec);

	Type valtype = val->getType();
	Type dpt = deep->getType();

	// If it's a user-defined type, replace by it's defintion.
	if (DEFINED_TYPE_NODE == dpt)
	{
		deep = DefineLink::get_definition(deep);
		dpt = deep->getType();
	}

	// If it's a signature, unpack it now.
	if (SIGNATURE_LINK == dpt)
	{
		deep = deep->getOutgoingAtom(0);
		dpt = deep->getType();
	}

	if (TYPE_NODE == dpt)
	{
		Type deeptype = TypeNodeCast(deep)->get_value();
		return (valtype == deeptype);
	}
	else if (TYPE_CHOICE == dpt)
	{
		for (const Handle& choice : deep->getOutgoingSet())
		{
			if (value_is_type(choice, val)) return true;
		}
		return false;
	}
	else if (FUZZY_LINK == dpt)
	{
		throw RuntimeException(TRACE_INFO,
			"Not implemented! TODO XXX FIXME");
	}

	// If it is a node, not a link, then it is a type-constant,
	// and thus must match perfectly.
	if (deep->isNode())
		return (deep == val);

	// If a link, then both must be same link type.
	if (valtype != dpt) return false;

	const HandleSeq& vlo = val->getOutgoingSet();
	const HandleSeq& dpo = deep->getOutgoingSet();
	size_t sz = dpo.size();

	// Both must be the same size...
	if (vlo.size() != sz) return false;

	// Unordered links are harder to handle...
	if (classserver().isA(dpt, UNORDERED_LINK))
		throw RuntimeException(TRACE_INFO,
			"Not implemented! TODO XXX FIXME");

	// Ordered links are compared side-by-side
	for (size_t i=0; i<sz; i++)
	{
		if (not value_is_type(dpo[i], vlo[i])) return false;
	}

	// If we are here, all checks must hav passed.
	return true;
}

/* ================================================================= */

/*
 * The implementation below feels awfully hacky and bug-prone,
 * but I don't see a better way.  Since below is essentially
 * a whizzy set-subset operation, one could go all formal operator
 * and etc. but more abstraction seems unlikely to make it better.
 */
static bool type_match_rec(const Handle& left_, const Handle& right_, bool toplevel)
{
	if (left_ == right_) return true;

	Handle left(left_);
	Type ltype = left->getType();

	// If it's a user-defined type, replace by it's defintion.
	if (DEFINED_TYPE_NODE == ltype)
	{
		left = DefineLink::get_definition(left);
		ltype = left->getType();
	}

	// Unpack the arrow; right must match left's input.
	if (ARROW_LINK == ltype)
	{
		left = left->getOutgoingAtom(0); // 0 == input
		ltype = left->getType();
	}

	// If right is not a type, then just use value-check.
	// We can only do this at the top level; lower levels
	// can have value-like links (i.e. duck-types which
	// we have to type-interence).
	Type rtype = right_->getType();
	if (toplevel and
	    TYPE_NODE != rtype and
	    TYPE_CHOICE != rtype and
	    SIGNATURE_LINK != rtype and
	    DEFINED_TYPE_NODE != rtype and
	    ARROW_LINK != rtype)
	{
		return value_is_type(left, right_);
	}

	Handle right(right_);

	// If it's a user-defined type, replace by it's defintion.
	if (DEFINED_TYPE_NODE == rtype)
	{
		right = DefineLink::get_definition(right);
		rtype = right->getType();
	}

	// Unpack the arrow; right's output must match left.
	if (ARROW_LINK == rtype)
	{
		right = right->getOutgoingAtom(1); // 1 == output
		rtype = right->getType();
	}

	// Should be safe to unpack signatures now.
	// We must not do this earlier, it will mess up.
	if (SIGNATURE_LINK == ltype)
	{
		left = left->getOutgoingAtom(0);
		ltype = left->getType();
	}

	if (SIGNATURE_LINK == rtype)
	{
		right = right->getOutgoingAtom(0);
		rtype = right->getType();
	}

	// Exact matchees are always good.
	if (left == right) return true;

	// If left is a core type, right must be that type
	// (or a value, partly handled that above already, for
	// the top-level; here we do lower levels.
	if (TYPE_NODE == ltype)
	{
		return TypeNodeCast(left)->get_value() == rtype;
	}

	// If left is a type choice, right must match a choice.
	if (TYPE_CHOICE == ltype)
	{
		if (TYPE_CHOICE == rtype)
		{
			// Can everything in the right be found in the left?
			// If so, then we are OK.
			for (const Handle& rh : right->getOutgoingSet())
			{
				if (not type_match_rec(left, rh, false)) return false;
			}
			return true;
		}

		// Does one of the left choices atch a right? Is so then good.
		for (const Handle& lh : left->getOutgoingSet())
		{
			if (type_match_rec(lh, right, false)) return true;
		}
		return false;
	}

	// At this point we expect both left an right to be links
	// that are not type-links, i.e. should be ordinary links,
	// e.g. ListLink or EvaluationLink.  Compare these side-by-side.
	if (ltype != rtype) return false;

	if (not left->isLink() or not right->isLink()) return false;

	// Unordered links are a pain in the butt.
	if (classserver().isA(ltype, UNORDERED_LINK))
		throw RuntimeException(TRACE_INFO,
			"Not implemented! TODO XXX FIXME");

	const HandleSeq& lout(left->getOutgoingSet());
	const HandleSeq& rout(right->getOutgoingSet());

	if (lout.size() != rout.size()) return false;

	for (size_t i=0; i< lout.size(); i++)
	{
		if (not type_match_rec(lout[i], rout[i], false)) return false;
	}

	return true;
}

bool type_match(const Handle& left_, const Handle& right_)
{
	return type_match_rec(left_, right_, true);
}

Handle type_compose(const Handle& left, const Handle& right)
{
	return Handle::UNDEFINED;
}

Handle filter_vardecl(const Handle& vardecl, const Handle& body)
{
	return filter_vardecl(vardecl, HandleSeq{body});
}

Handle filter_vardecl(const Handle& vardecl, const HandleSeq& hs)
{
	// Base cases

	if (vardecl.is_undefined())
		return Handle::UNDEFINED;  // XXX TODO .. return?? or throw?

	Type t = vardecl->getType();
	if (VARIABLE_NODE == t)
	{
		if (is_free_in_any_tree(hs, vardecl))
			return vardecl;
	}

	// Recursive cases

	else if (TYPED_VARIABLE_LINK == t)
	{
		if (filter_vardecl(vardecl->getOutgoingAtom(0), hs).is_defined())
			return vardecl;
	}

	else if (VARIABLE_LIST == t)
	{
		HandleSeq subvardecls;
		for (const Handle& v : vardecl->getOutgoingSet())
		{
			if (filter_vardecl(v, hs).is_defined())
				subvardecls.push_back(v);
		}
		if (subvardecls.empty())
			return Handle::UNDEFINED;
		if (subvardecls.size() == 1)
			return subvardecls[0];
		return Handle(createVariableList(subvardecls));
	}

	// XXX TODO .. undefined?? or throw?
	return Handle::UNDEFINED;
}

Type type_intersection(Type lhs, Type rhs)
{
	ClassServer& cs = classserver();
	if (cs.isA(lhs, rhs))
		return lhs;
	if (cs.isA(rhs, lhs))
		return rhs;
	return NOTYPE;              // represent the bottom type
}

std::set<Type> type_intersection(Type lhs, const std::set<Type>& rhs)
{
	std::set<Type> res;
	// Distribute the intersection over the union type rhs
	for (Type rhst : rhs) {
		Type ty = type_intersection(lhs, rhst);
		if (ty != NOTYPE)
			res.insert(ty);
	}
	return res;
}

std::set<Type> type_intersection(const std::set<Type>& lhs,
                                 const std::set<Type>& rhs)
{
	// Base cases
	if (lhs.empty())
		return rhs;
	if (rhs.empty())
		return lhs;

	// Recursive cases
	std::set<Type> res;
	for (Type ty : lhs) {
		std::set<Type> itr = type_intersection(ty, rhs);
		res.insert(itr.begin(), itr.end());
	}
	return res;
}

} // ~namespace opencog

/* ===================== END OF FILE ===================== */
<commit_msg>Fix filter_vardecl, first arg of TypeVariable must be a variable<commit_after>/*
 * TypeUtils.cc
 *
 * Copyright (C) 2015 Linas Vepstas
 *
 * Author: Linas Vepstas <linasvepstas@gmail.com>  December 2015
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License v3 as
 * published by the Free Software Foundation and including the
 * exceptions at http://opencog.org/wiki/Licenses
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public
 * License along with this program; if not, write to:
 * Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <opencog/atoms/base/Link.h>
#include <opencog/atoms/base/ClassServer.h>

#include <opencog/atoms/TypeNode.h>
#include <opencog/atoms/core/DefineLink.h>
#include <opencog/atoms/core/VariableList.h>

#include "FindUtils.h"

#include "TypeUtils.h"

namespace opencog {

/* ================================================================= */
/**
 * Type checker.  Returns true if `val` is of type `deep`.
 */
bool value_is_type(const Handle& spec, const Handle& val)
{
	Handle deep(spec);

	Type valtype = val->getType();
	Type dpt = deep->getType();

	// If it's a user-defined type, replace by it's defintion.
	if (DEFINED_TYPE_NODE == dpt)
	{
		deep = DefineLink::get_definition(deep);
		dpt = deep->getType();
	}

	// If it's a signature, unpack it now.
	if (SIGNATURE_LINK == dpt)
	{
		deep = deep->getOutgoingAtom(0);
		dpt = deep->getType();
	}

	if (TYPE_NODE == dpt)
	{
		Type deeptype = TypeNodeCast(deep)->get_value();
		return (valtype == deeptype);
	}
	else if (TYPE_CHOICE == dpt)
	{
		for (const Handle& choice : deep->getOutgoingSet())
		{
			if (value_is_type(choice, val)) return true;
		}
		return false;
	}
	else if (FUZZY_LINK == dpt)
	{
		throw RuntimeException(TRACE_INFO,
			"Not implemented! TODO XXX FIXME");
	}

	// If it is a node, not a link, then it is a type-constant,
	// and thus must match perfectly.
	if (deep->isNode())
		return (deep == val);

	// If a link, then both must be same link type.
	if (valtype != dpt) return false;

	const HandleSeq& vlo = val->getOutgoingSet();
	const HandleSeq& dpo = deep->getOutgoingSet();
	size_t sz = dpo.size();

	// Both must be the same size...
	if (vlo.size() != sz) return false;

	// Unordered links are harder to handle...
	if (classserver().isA(dpt, UNORDERED_LINK))
		throw RuntimeException(TRACE_INFO,
			"Not implemented! TODO XXX FIXME");

	// Ordered links are compared side-by-side
	for (size_t i=0; i<sz; i++)
	{
		if (not value_is_type(dpo[i], vlo[i])) return false;
	}

	// If we are here, all checks must hav passed.
	return true;
}

/* ================================================================= */

/*
 * The implementation below feels awfully hacky and bug-prone,
 * but I don't see a better way.  Since below is essentially
 * a whizzy set-subset operation, one could go all formal operator
 * and etc. but more abstraction seems unlikely to make it better.
 */
static bool type_match_rec(const Handle& left_, const Handle& right_, bool toplevel)
{
	if (left_ == right_) return true;

	Handle left(left_);
	Type ltype = left->getType();

	// If it's a user-defined type, replace by it's defintion.
	if (DEFINED_TYPE_NODE == ltype)
	{
		left = DefineLink::get_definition(left);
		ltype = left->getType();
	}

	// Unpack the arrow; right must match left's input.
	if (ARROW_LINK == ltype)
	{
		left = left->getOutgoingAtom(0); // 0 == input
		ltype = left->getType();
	}

	// If right is not a type, then just use value-check.
	// We can only do this at the top level; lower levels
	// can have value-like links (i.e. duck-types which
	// we have to type-interence).
	Type rtype = right_->getType();
	if (toplevel and
	    TYPE_NODE != rtype and
	    TYPE_CHOICE != rtype and
	    SIGNATURE_LINK != rtype and
	    DEFINED_TYPE_NODE != rtype and
	    ARROW_LINK != rtype)
	{
		return value_is_type(left, right_);
	}

	Handle right(right_);

	// If it's a user-defined type, replace by it's defintion.
	if (DEFINED_TYPE_NODE == rtype)
	{
		right = DefineLink::get_definition(right);
		rtype = right->getType();
	}

	// Unpack the arrow; right's output must match left.
	if (ARROW_LINK == rtype)
	{
		right = right->getOutgoingAtom(1); // 1 == output
		rtype = right->getType();
	}

	// Should be safe to unpack signatures now.
	// We must not do this earlier, it will mess up.
	if (SIGNATURE_LINK == ltype)
	{
		left = left->getOutgoingAtom(0);
		ltype = left->getType();
	}

	if (SIGNATURE_LINK == rtype)
	{
		right = right->getOutgoingAtom(0);
		rtype = right->getType();
	}

	// Exact matchees are always good.
	if (left == right) return true;

	// If left is a core type, right must be that type
	// (or a value, partly handled that above already, for
	// the top-level; here we do lower levels.
	if (TYPE_NODE == ltype)
	{
		return TypeNodeCast(left)->get_value() == rtype;
	}

	// If left is a type choice, right must match a choice.
	if (TYPE_CHOICE == ltype)
	{
		if (TYPE_CHOICE == rtype)
		{
			// Can everything in the right be found in the left?
			// If so, then we are OK.
			for (const Handle& rh : right->getOutgoingSet())
			{
				if (not type_match_rec(left, rh, false)) return false;
			}
			return true;
		}

		// Does one of the left choices atch a right? Is so then good.
		for (const Handle& lh : left->getOutgoingSet())
		{
			if (type_match_rec(lh, right, false)) return true;
		}
		return false;
	}

	// At this point we expect both left an right to be links
	// that are not type-links, i.e. should be ordinary links,
	// e.g. ListLink or EvaluationLink.  Compare these side-by-side.
	if (ltype != rtype) return false;

	if (not left->isLink() or not right->isLink()) return false;

	// Unordered links are a pain in the butt.
	if (classserver().isA(ltype, UNORDERED_LINK))
		throw RuntimeException(TRACE_INFO,
			"Not implemented! TODO XXX FIXME");

	const HandleSeq& lout(left->getOutgoingSet());
	const HandleSeq& rout(right->getOutgoingSet());

	if (lout.size() != rout.size()) return false;

	for (size_t i=0; i< lout.size(); i++)
	{
		if (not type_match_rec(lout[i], rout[i], false)) return false;
	}

	return true;
}

bool type_match(const Handle& left_, const Handle& right_)
{
	return type_match_rec(left_, right_, true);
}

Handle type_compose(const Handle& left, const Handle& right)
{
	return Handle::UNDEFINED;
}

Handle filter_vardecl(const Handle& vardecl, const Handle& body)
{
	return filter_vardecl(vardecl, HandleSeq{body});
}

Handle filter_vardecl(const Handle& vardecl, const HandleSeq& hs)
{
	// Base cases

	if (vardecl.is_undefined())
		// Return Handle::UNDEFINED to indicate that this variable
		// declaration is useless.
		return Handle::UNDEFINED;

	Type t = vardecl->getType();
	if (VARIABLE_NODE == t)
	{
		if (is_free_in_any_tree(hs, vardecl))
			return vardecl;
	}

	// Recursive cases

	else if (TYPED_VARIABLE_LINK == t)
	{
		Handle var = vardecl->getOutgoingAtom(0);
		Type t = var->getType();
		if (t == VARIABLE_NODE and filter_vardecl(var, hs).is_defined())
			return vardecl;
	}

	else if (VARIABLE_LIST == t)
	{
		HandleSeq subvardecls;
		for (const Handle& v : vardecl->getOutgoingSet())
		{
			if (filter_vardecl(v, hs).is_defined())
				subvardecls.push_back(v);
		}
		if (subvardecls.empty())
			return Handle::UNDEFINED;
		if (subvardecls.size() == 1)
			return subvardecls[0];
		return Handle(createVariableList(subvardecls));
	}

	// If we're here we have failed to recognize vardecl as a useful
	// and well formed variable declaration, so Handle::UNDEFINED is
	// returned.
	return Handle::UNDEFINED;
}

Type type_intersection(Type lhs, Type rhs)
{
	ClassServer& cs = classserver();
	if (cs.isA(lhs, rhs))
		return lhs;
	if (cs.isA(rhs, lhs))
		return rhs;
	return NOTYPE;              // represent the bottom type
}

std::set<Type> type_intersection(Type lhs, const std::set<Type>& rhs)
{
	std::set<Type> res;
	// Distribute the intersection over the union type rhs
	for (Type rhst : rhs) {
		Type ty = type_intersection(lhs, rhst);
		if (ty != NOTYPE)
			res.insert(ty);
	}
	return res;
}

std::set<Type> type_intersection(const std::set<Type>& lhs,
                                 const std::set<Type>& rhs)
{
	// Base cases
	if (lhs.empty())
		return rhs;
	if (rhs.empty())
		return lhs;

	// Recursive cases
	std::set<Type> res;
	for (Type ty : lhs) {
		std::set<Type> itr = type_intersection(ty, rhs);
		res.insert(itr.begin(), itr.end());
	}
	return res;
}

} // ~namespace opencog

/* ===================== END OF FILE ===================== */
<|endoftext|>


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

#include <iostream>
#include <fstream>
#include "compiler/compiler.h"
#include "compiler/cstring.h"
#include "modules/std/net/tcpsocket.h"
#include "types/nativetypes.h"

namespace clever { namespace packages { namespace std { namespace net {

CLEVER_METHOD(TcpSocket::constructor) {
	SocketValue* sv = new SocketValue;

	if (args != NULL) {
		if (CLEVER_NUM_ARGS() == 1) {
			// Host only.
			sv->getSocket()->setHost(CLEVER_ARG_STR(0).c_str());
		} else if (CLEVER_NUM_ARGS() == 2) {
			// Host and port.
			sv->getSocket()->setHost(CLEVER_ARG_STR(0).c_str());
			sv->getSocket()->setPort(CLEVER_ARG_INT(1));
		}
	}

	CLEVER_RETURN_DATA_VALUE(sv);
}

CLEVER_METHOD(TcpSocket::setHost) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	sv->getSocket()->setHost(CLEVER_ARG_STR(0).c_str());
}

CLEVER_METHOD(TcpSocket::setPort) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	sv->getSocket()->setPort(CLEVER_ARG_INT(0));
}

CLEVER_METHOD(TcpSocket::setTimeout) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	sv->getSocket()->setTimeout(CLEVER_ARG_INT(0));
}

CLEVER_METHOD(TcpSocket::connect) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	sv->getSocket()->connect();
}

CLEVER_METHOD(TcpSocket::close) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	sv->getSocket()->close();
}

CLEVER_METHOD(TcpSocket::receive) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);
	ValueVector* vv = new ValueVector;
	char *buffer;
	unsigned int length;

	length = CLEVER_ARG_INT(0);

	// Allocate the buffer.
	buffer = new char[length];
	memset(buffer, 0, length);

	// Receive the data.
	sv->getSocket()->receive(buffer, length);

	for (int i = 0; i < length; i++) {
		Value *v = new Value();
		v->setByte(buffer[i]);
		vv->push_back(v);
	}

	// Free the buffer.
	delete buffer;

	retval->setTypePtr(CLEVER_TYPE("Array<Byte>"));
	CLEVER_RETURN_ARRAY(vv);
}

CLEVER_METHOD(TcpSocket::send) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);
	ValueVector *vv = CLEVER_ARG_ARRAY(0);
	char *buffer;
	int bufferSize;

	// Allocate a buffer and fill it with the bytes from the array.
	buffer = new char[vv->size()];
	for (int i = 0; i < vv->size(); i++) {
		buffer[i] = static_cast<char>(vv->at(i)->getByte());
	}
	bufferSize = vv->size();

	// Send the data.
	sv->getSocket()->send(buffer, bufferSize);

	// Free the buffer.
	delete buffer;
}

CLEVER_METHOD(TcpSocket::isOpen) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	CLEVER_RETURN_BOOL(sv->getSocket()->isOpen());
}

CLEVER_METHOD(TcpSocket::poll) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	CLEVER_RETURN_BOOL(sv->getSocket()->poll());
}

CLEVER_METHOD(TcpSocket::good) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	CLEVER_RETURN_BOOL(sv->getSocket()->getError() == NO_ERROR);
}

CLEVER_METHOD(TcpSocket::getError) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	CLEVER_RETURN_INT(sv->getSocket()->getError());
}

CLEVER_METHOD(TcpSocket::getErrorMessage) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	CLEVER_RETURN_STR(CSTRING(sv->getSocket()->getErrorString()));
}

CLEVER_METHOD(TcpSocket::toString) {
	CLEVER_RETURN_STR(CSTRING("TcpSocket class"));
}

/**
 * Void TcpSocket::__assign__(TcpSocket)
 */
CLEVER_METHOD(TcpSocket::do_assign) {
	CLEVER_ARG(0)->getDataValue()->addRef();
	CLEVER_THIS()->copy(CLEVER_ARG(0));
}

void TcpSocket::init() {
	const Type* tcpsock = CLEVER_TYPE("TcpSocket");
	const Type* arr_byte = CLEVER_GET_ARRAY_TEMPLATE->getTemplatedType(CLEVER_BYTE);

	addMethod(
		(new Method(CLEVER_CTOR_NAME, (MethodPtr)&TcpSocket::do_assign, tcpsock))
			->addArg("rvalue", tcpsock)
	);

	addMethod(new Method(CLEVER_CTOR_NAME, (MethodPtr)&TcpSocket::constructor, tcpsock));

	addMethod(
		(new Method(CLEVER_CTOR_NAME, (MethodPtr)&TcpSocket::constructor, tcpsock))
			->addArg("host", CLEVER_STR)
	);

	addMethod(
		(new Method(CLEVER_CTOR_NAME, (MethodPtr)&TcpSocket::constructor, tcpsock))
			->addArg("host", CLEVER_STR)
			->addArg("port", CLEVER_INT)
	);

	addMethod(
		(new Method("setHost", (MethodPtr)&TcpSocket::setHost, CLEVER_VOID))
			->addArg("host", CLEVER_STR)
	);

	addMethod(
		(new Method("setPort", (MethodPtr)&TcpSocket::setPort, CLEVER_VOID))
			->addArg("port", CLEVER_INT)
	);

	addMethod(
		(new Method("setTimeout", (MethodPtr)&TcpSocket::setTimeout, CLEVER_VOID))
			->addArg("timeout", CLEVER_INT)
	);

	addMethod(new Method("connect", (MethodPtr)&TcpSocket::connect, tcpsock));
	addMethod(new Method("close", (MethodPtr)&TcpSocket::close, tcpsock));

	addMethod(
		(new Method("receive", (MethodPtr)&TcpSocket::receive, arr_byte))
			->addArg("length", CLEVER_INT)
	);

	addMethod(
		(new Method("send", (MethodPtr)&TcpSocket::send, CLEVER_VOID))
			->addArg("data", arr_byte)
	);

	addMethod(new Method("isOpen", (MethodPtr)&TcpSocket::isOpen, CLEVER_BOOL));
	addMethod(new Method("poll", (MethodPtr)&TcpSocket::poll, CLEVER_BOOL));

	addMethod(new Method("good", (MethodPtr)&TcpSocket::good, CLEVER_BOOL));
	addMethod(new Method("getError", (MethodPtr)&TcpSocket::getError, CLEVER_INT));
	addMethod(new Method("getErrorMessage", (MethodPtr)&TcpSocket::getErrorMessage, CLEVER_INT));

	addMethod(new Method("toString", (MethodPtr)&TcpSocket::toString, CLEVER_STR));

	// Windows socket initialization.
#ifdef CLEVER_WIN32
	WSADATA wsaData;
	int res = WSAStartup(MAKEWORD(2, 2), &wsaData);
	if (res != NO_ERROR) {
		::std::cerr << "WSAStartup() failed: " << res << ::std::endl;
	}
#endif
}

DataValue* TcpSocket::allocateValue() const {
	return new SocketValue;
}

void TcpSocket::destructor(Value* value) const {
	//SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	// @TODO: disconnect.
}

}}}} // clever::packages::std::net
<commit_msg>Fix for *nix and other basic stuff.<commit_after>/**
 * Clever programming language
 * Copyright (c) 2012 Clever Team
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include <iostream>
#include <fstream>
#include "compiler/compiler.h"
#include "compiler/cstring.h"
#include "modules/std/net/tcpsocket.h"
#include "types/nativetypes.h"

namespace clever { namespace packages { namespace std { namespace net {

CLEVER_METHOD(TcpSocket::constructor) {
	SocketValue* sv = new SocketValue;

	if (args != NULL) {
		if (CLEVER_NUM_ARGS() == 1) {
			// Host only.
			sv->getSocket()->setHost(CLEVER_ARG_STR(0).c_str());
		} else if (CLEVER_NUM_ARGS() == 2) {
			// Host and port.
			sv->getSocket()->setHost(CLEVER_ARG_STR(0).c_str());
			sv->getSocket()->setPort(CLEVER_ARG_INT(1));
		}
	}

	CLEVER_RETURN_DATA_VALUE(sv);
}

CLEVER_METHOD(TcpSocket::setHost) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	sv->getSocket()->setHost(CLEVER_ARG_STR(0).c_str());
}

CLEVER_METHOD(TcpSocket::setPort) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	sv->getSocket()->setPort(CLEVER_ARG_INT(0));
}

CLEVER_METHOD(TcpSocket::setTimeout) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	sv->getSocket()->setTimeout(CLEVER_ARG_INT(0));
}

CLEVER_METHOD(TcpSocket::connect) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	sv->getSocket()->connect();
}

CLEVER_METHOD(TcpSocket::close) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	sv->getSocket()->close();
}

CLEVER_METHOD(TcpSocket::receive) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);
	ValueVector* vv = new ValueVector;
	char *buffer;
	int length;

	length = CLEVER_ARG_INT(0);

	// Allocate the buffer.
	buffer = new char[length];
	memset(buffer, 0, length);

	// Receive the data.
	sv->getSocket()->receive(buffer, length);

	for (int i = 0; i < length; ++i) {
		Value *v = new Value();
		v->setByte(buffer[i]);
		vv->push_back(v);
	}

	// Free the buffer.
	delete buffer;

	retval->setTypePtr(CLEVER_TYPE("Array<Byte>"));
	CLEVER_RETURN_ARRAY(vv);
}

CLEVER_METHOD(TcpSocket::send) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);
	ValueVector *vv = CLEVER_ARG_ARRAY(0);
	char *buffer;
	int bufferSize;

	// Allocate a buffer and fill it with the bytes from the array.
	buffer = new char[vv->size()];
	for (size_t i = 0; i < vv->size(); ++i) {
		buffer[i] = static_cast<char>(vv->at(i)->getByte());
	}
	bufferSize = vv->size();

	// Send the data.
	sv->getSocket()->send(buffer, bufferSize);

	// Free the buffer.
	delete buffer;
}

CLEVER_METHOD(TcpSocket::isOpen) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	CLEVER_RETURN_BOOL(sv->getSocket()->isOpen());
}

CLEVER_METHOD(TcpSocket::poll) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	CLEVER_RETURN_BOOL(sv->getSocket()->poll());
}

CLEVER_METHOD(TcpSocket::good) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	CLEVER_RETURN_BOOL(sv->getSocket()->getError() == NO_ERROR);
}

CLEVER_METHOD(TcpSocket::getError) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	CLEVER_RETURN_INT(sv->getSocket()->getError());
}

CLEVER_METHOD(TcpSocket::getErrorMessage) {
	SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	CLEVER_RETURN_STR(CSTRING(sv->getSocket()->getErrorString()));
}

CLEVER_METHOD(TcpSocket::toString) {
	CLEVER_RETURN_STR(CSTRING("TcpSocket class"));
}

/**
 * Void TcpSocket::__assign__(TcpSocket)
 */
CLEVER_METHOD(TcpSocket::do_assign) {
	CLEVER_ARG(0)->getDataValue()->addRef();
	CLEVER_THIS()->copy(CLEVER_ARG(0));
}

void TcpSocket::init() {
	const Type* tcpsock = CLEVER_TYPE("TcpSocket");
	const Type* arr_byte = CLEVER_GET_ARRAY_TEMPLATE->getTemplatedType(CLEVER_BYTE);

	addMethod(
		(new Method(CLEVER_CTOR_NAME, (MethodPtr)&TcpSocket::do_assign, tcpsock))
			->addArg("rvalue", tcpsock)
	);

	addMethod(new Method(CLEVER_CTOR_NAME, (MethodPtr)&TcpSocket::constructor, tcpsock));

	addMethod(
		(new Method(CLEVER_CTOR_NAME, (MethodPtr)&TcpSocket::constructor, tcpsock))
			->addArg("host", CLEVER_STR)
	);

	addMethod(
		(new Method(CLEVER_CTOR_NAME, (MethodPtr)&TcpSocket::constructor, tcpsock))
			->addArg("host", CLEVER_STR)
			->addArg("port", CLEVER_INT)
	);

	addMethod(
		(new Method("setHost", (MethodPtr)&TcpSocket::setHost, CLEVER_VOID))
			->addArg("host", CLEVER_STR)
	);

	addMethod(
		(new Method("setPort", (MethodPtr)&TcpSocket::setPort, CLEVER_VOID))
			->addArg("port", CLEVER_INT)
	);

	addMethod(
		(new Method("setTimeout", (MethodPtr)&TcpSocket::setTimeout, CLEVER_VOID))
			->addArg("timeout", CLEVER_INT)
	);

	addMethod(new Method("connect", (MethodPtr)&TcpSocket::connect, tcpsock));
	addMethod(new Method("close", (MethodPtr)&TcpSocket::close, tcpsock));

	addMethod(
		(new Method("receive", (MethodPtr)&TcpSocket::receive, arr_byte))
			->addArg("length", CLEVER_INT)
	);

	addMethod(
		(new Method("send", (MethodPtr)&TcpSocket::send, CLEVER_VOID))
			->addArg("data", arr_byte)
	);

	addMethod(new Method("isOpen", (MethodPtr)&TcpSocket::isOpen, CLEVER_BOOL));
	addMethod(new Method("poll", (MethodPtr)&TcpSocket::poll, CLEVER_BOOL));

	addMethod(new Method("good", (MethodPtr)&TcpSocket::good, CLEVER_BOOL));
	addMethod(new Method("getError", (MethodPtr)&TcpSocket::getError, CLEVER_INT));
	addMethod(new Method("getErrorMessage", (MethodPtr)&TcpSocket::getErrorMessage, CLEVER_INT));

	addMethod(new Method("toString", (MethodPtr)&TcpSocket::toString, CLEVER_STR));

	// Windows socket initialization.
#ifdef CLEVER_WIN32
	WSADATA wsaData;
	int res = WSAStartup(MAKEWORD(2, 2), &wsaData);
	if (res != NO_ERROR) {
		::std::cerr << "WSAStartup() failed: " << res << ::std::endl;
	}
#endif
}

DataValue* TcpSocket::allocateValue() const {
	return new SocketValue;
}

void TcpSocket::destructor(Value* value) const {
	//SocketValue* sv = CLEVER_GET_VALUE(SocketValue*, value);

	// @TODO: disconnect.
}

}}}} // clever::packages::std::net
<|endoftext|>


<commit_before>#include "Checksum.hpp"

namespace Util
{
    namespace Checksum
    {
        uint16_t Crc16(const void* Data, size_t Length, uint16_t PrevCrc)
        {
            const uint16_t Polynomial = 0x8408;
            uint16_t Crc = ~PrevCrc;
            uint8_t* Current = (uint8_t*)Data;
            while( Length-- )
            {
                Crc ^= *Current++;
                for( uint8_t i = 0; i < 8; i++ )
                {
                    Crc = (Crc >> 1) ^ (-int16_t(Crc & 1) & Polynomial);
                }
            }
            return ~Crc;
        }

        uint32_t Crc32(const void* Data, size_t Length, uint32_t PrevCrc)
        {
            const uint32_t Polynomial = 0xEDB88320;
            uint32_t Crc = ~PrevCrc;
            uint8_t* Current = (uint8_t*)Data;
            while( Length-- )
            {
                Crc ^= *Current++;
                for( uint8_t i = 0; i < 8; i++ )
                {
                    Crc = (Crc >> 1) ^ (-int32_t(Crc & 1) & Polynomial);
                }
            }

            return ~Crc;
        }

        uint16_t Fletcher16(const void* Data, size_t Length)
        {
            register uint16_t sum1, sum2;
            sum1 = sum2 = 0;
            for( size_t i = 0; i < Length; ++i )
            {
                sum1 += static_cast<const uint8_t*>(Data)[i];
                sum1 %= 0xff;
                sum2 += sum1;
                sum2 %= 0xff;
            }
            return (sum2 << 8) | sum1;
        }

        uint16_t Bsd16(const void* Data, size_t Length)
        {
            uint16_t Checksum;
            for( size_t i = 0; i < Length; i++ )
            {
                Checksum = (Checksum >> 1) | ((Checksum & 1) << 15);
                Checksum += static_cast<const uint8_t*>(Data)[i];
                Checksum &= 0xffff;
            }
            return Checksum;
        }

        uint32_t Fnv32(const void* Data, size_t Length)
        {
            const uint32_t OffsetBasis = 2166136261u;
            const uint32_t Prime = 16777619u;

            uint32_t Checksum = OffsetBasis;
            for( size_t i = 0; i < Length; i++ )
            {
                Checksum *= Prime;
                Checksum ^= static_cast<const uint8_t*>(Data)[i];
            }
            return Checksum;
        }

        uint32_t FnvA32(const void* Data, size_t Length)
        {
            const uint32_t OffsetBasis = 2166136261u;
            const uint32_t Prime = 16777619u;

            uint32_t Checksum = OffsetBasis;
            for( size_t i = 0; i < Length; i++ )
            {
                Checksum ^= static_cast<const uint8_t*>(Data)[i];
                Checksum *= Prime;
            }
            return Checksum;
        }

        uint64_t Fnv64(const void* Data, size_t Length)
        {
            const uint64_t OffsetBasis = 14695981039346656037u;
            const uint64_t Prime = 1099511628211u;

            uint64_t Checksum = OffsetBasis;
            for( size_t i = 0; i < Length; i++ )
            {
                Checksum *= Prime;
                Checksum ^= static_cast<const uint8_t*>(Data)[i];
            }
            return Checksum;
        }

        uint64_t FnvA64(const void* Data, size_t Length)
        {
            const uint64_t OffsetBasis = 14695981039346656037u;
            const uint64_t Prime = 1099511628211u;

            uint64_t Checksum = OffsetBasis;
            for( size_t i = 0; i < Length; i++ )
            {
                Checksum ^= static_cast<const uint8_t*>(Data)[i];
                Checksum *= Prime;
            }
            return Checksum;
        }

        uint32_t Murmur32(const void* Data, uint32_t Length, uint32_t Seed)
        {
            static const uint32_t C1 = 0xCC9E2D51;
            static const uint32_t C2 = 0x1B873593;
            static const uint32_t R1 = 15;
            static const uint32_t R2 = 13;
            static const uint32_t M = 5;
            static const uint32_t N = 0xE6546B64;
            uint32_t K1 = 0;

            uint32_t Hash = Seed;
            uint32_t Chunks = Length >> 2;

            // Process all 4 byte chunks
            for( size_t i = K1 = 0; i < Chunks; i++ )
            {
                uint32_t K = static_cast<const uint32_t*>(Data)[i];
                K *= C1;
                K = (K << R1) | (K >> (32 - R1));
                K *= C2;

                Hash ^= K;
                Hash = ((Hash << R2) | (Hash >> (32 - R2)));
                Hash *= M + N;
            }

            // Remaining bytes
            const uint32_t *End = static_cast<const uint32_t*>(Data) + Chunks * 4;

            switch( Length & 3 )
            {
            case 3:
            {
                Hash ^= End[2] << 16;
                break;
            }
            case 2:
            {
                K1 ^= End[1] << 8;
                break;
            }
            case 1:
            {
                K1 ^= End[0];
                K1 = (K1 << R1) | (K1 >> (32 - R1));
                K1 *= C2;
                Hash ^= K1;
                break;
            }
            default:
                break;
            }

            Hash ^= Length;
            Hash ^= (Hash >> 16);
            Hash *= 0x85EBCA6B;
            Hash ^= (Hash >> 13);
            Hash *= 0xC2B2AE35;
            Hash ^= (Hash >> 16);

            return 0;
        }
    }
}<commit_msg>Checksum refactor<commit_after>#include "Checksum.hpp"

namespace Util
{
    namespace Checksum
    {
        uint16_t Crc16(const void* Data, size_t Length, uint16_t PrevCrc)
        {
            const uint16_t Polynomial = 0x8408;
            uint16_t Crc = ~PrevCrc;
            const uint8_t* Current = reinterpret_cast<const uint8_t*>(Data);
            while( Length-- )
            {
                Crc ^= *Current++;
                for( uint8_t i = 0; i < 8; i++ )
                {
                    Crc = (Crc >> 1) ^ (-static_cast<int16_t>(Crc & 1) & Polynomial);
                }
            }
            return ~Crc;
        }

        uint32_t Crc32(const void* Data, size_t Length, uint32_t PrevCrc)
        {
            const uint32_t Polynomial = 0xEDB88320;
            uint32_t Crc = ~PrevCrc;
            const uint8_t* Current = reinterpret_cast<const uint8_t*>(Data);
            while( Length-- )
            {
                Crc ^= *Current++;
                for( uint8_t i = 0; i < 8; i++ )
                {
                    Crc = (Crc >> 1) ^ (-static_cast<int32_t>(Crc & 1) & Polynomial);
                }
            }

            return ~Crc;
        }

        uint16_t Fletcher16(const void* Data, size_t Length)
        {
            uint16_t sum1, sum2;
            sum1 = sum2 = 0;
            for( size_t i = 0; i < Length; ++i )
            {
                sum1 += static_cast<const uint8_t*>(Data)[i];
                sum1 %= 0xff;
                sum2 += sum1;
                sum2 %= 0xff;
            }
            return (sum2 << 8) | sum1;
        }

        uint16_t Bsd16(const void* Data, size_t Length)
        {
            uint16_t Checksum;
            for( size_t i = 0; i < Length; i++ )
            {
                Checksum = (Checksum >> 1) | ((Checksum & 1) << 15);
                Checksum += static_cast<const uint8_t*>(Data)[i];
                Checksum &= 0xffff;
            }
            return Checksum;
        }

        uint32_t Fnv32(const void* Data, size_t Length)
        {
            const uint32_t OffsetBasis = 2166136261u;
            const uint32_t Prime = 16777619u;

            uint32_t Checksum = OffsetBasis;
            for( size_t i = 0; i < Length; i++ )
            {
                Checksum *= Prime;
                Checksum ^= static_cast<const uint8_t*>(Data)[i];
            }
            return Checksum;
        }

        uint32_t FnvA32(const void* Data, size_t Length)
        {
            const uint32_t OffsetBasis = 2166136261u;
            const uint32_t Prime = 16777619u;

            uint32_t Checksum = OffsetBasis;
            for( size_t i = 0; i < Length; i++ )
            {
                Checksum ^= static_cast<const uint8_t*>(Data)[i];
                Checksum *= Prime;
            }
            return Checksum;
        }

        uint64_t Fnv64(const void* Data, size_t Length)
        {
            const uint64_t OffsetBasis = 14695981039346656037u;
            const uint64_t Prime = 1099511628211u;

            uint64_t Checksum = OffsetBasis;
            for( size_t i = 0; i < Length; i++ )
            {
                Checksum *= Prime;
                Checksum ^= static_cast<const uint8_t*>(Data)[i];
            }
            return Checksum;
        }

        uint64_t FnvA64(const void* Data, size_t Length)
        {
            const uint64_t OffsetBasis = 14695981039346656037u;
            const uint64_t Prime = 1099511628211u;

            uint64_t Checksum = OffsetBasis;
            for( size_t i = 0; i < Length; i++ )
            {
                Checksum ^= static_cast<const uint8_t*>(Data)[i];
                Checksum *= Prime;
            }
            return Checksum;
        }

        uint32_t Murmur32(const void* Data, uint32_t Length, uint32_t Seed)
        {
            const uint32_t C1 = 0xCC9E2D51;
            const uint32_t C2 = 0x1B873593;
            const uint32_t R1 = 15;
            const uint32_t R2 = 13;
            const uint32_t M = 5;
            const uint32_t N = 0xE6546B64;
            uint32_t K1 = 0;

            uint32_t Hash = Seed;
            uint32_t Chunks = Length >> 2;

            // Process all 4 byte chunks
            for( size_t i = K1 = 0; i < Chunks; i++ )
            {
                uint32_t K = static_cast<const uint32_t*>(Data)[i];
                K *= C1;
                K = (K << R1) | (K >> (32 - R1));
                K *= C2;

                Hash ^= K;
                Hash = ((Hash << R2) | (Hash >> (32 - R2)));
                Hash *= M + N;
            }

            // Remaining bytes
            const uint32_t *End = static_cast<const uint32_t*>(Data) + Chunks * 4;

            switch( Length & 3 )
            {
            case 3:
            {
                Hash ^= End[2] << 16;
                break;
            }
            case 2:
            {
                K1 ^= End[1] << 8;
                break;
            }
            case 1:
            {
                K1 ^= End[0];
                K1 = (K1 << R1) | (K1 >> (32 - R1));
                K1 *= C2;
                Hash ^= K1;
                break;
            }
            default:
                break;
            }

            Hash ^= Length;
            Hash ^= (Hash >> 16);
            Hash *= 0x85EBCA6B;
            Hash ^= (Hash >> 13);
            Hash *= 0xC2B2AE35;
            Hash ^= (Hash >> 16);

            return 0;
        }
    }
}<|endoftext|>