Skip to content

Instantly share code, notes, and snippets.

@gocarlos

gocarlos/CalculateCenterOfMassFromMesh.cpp

Last active February 28, 2017 15:42
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save gocarlos/029aaa003c465704d5f04bbbecaedcc8 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save gocarlos/029aaa003c465704d5f04bbbecaedcc8 to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. gocarlos revised this gist Feb 28, 2017. No changes.
  2. gocarlos created this gist Feb 28, 2017.
    103 changes: 103 additions & 0 deletions CalculateCenterOfMassFromMesh.cpp
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,103 @@
    // Reference: http://mathworld.wolfram.com/Tetrahedron.html
    void Common::CalculateObjectsProperties(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
    Eigen::Vector3d* center_of_mass, double* totalVolume, double* weight) {
    LOG(INFO) << "CalculateObjectsProperties start.\n";

    CHECK_NOTNULL(cloud.get());
    CHECK_NOTNULL(center_of_mass);
    CHECK_NOTNULL(totalVolume);
    CHECK_NOTNULL(weight);

    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
    for (size_t i = 0; i < cloud->points.size(); ++i) {
    points->InsertNextPoint(cloud->points[i].x, cloud->points[i].y,
    cloud->points[i].z);
    }
    vtkSmartPointer<vtkPolyData> original_vtk_mesh =
    vtkSmartPointer<vtkPolyData>::New();
    original_vtk_mesh->SetPoints(points);

    vtkSmartPointer<vtkPolyData> convex_hull =
    vtkSmartPointer<vtkPolyData>::New();
    CalculateConvexHull(original_vtk_mesh, convex_hull);

    LOG(INFO) << "Number of cells " << convex_hull->GetNumberOfCells();
    LOG(INFO) << "Number of points " << convex_hull->GetNumberOfPoints();
    LOG(INFO) << "Number of polys " << convex_hull->GetNumberOfPolys();

    int numTriangles = convex_hull->GetNumberOfCells();
    LOG(INFO) << "Number of triangles is " << numTriangles;
    CHECK_GT(numTriangles, 0);

    /*!
    * Struct which defines a triangle in a mesh.
    */
    struct Triangle // 3 vertices of each triangle
    {
    float x1, y1, z1;
    float x2, y2, z2;
    float x3, y3, z3;
    };

    Triangle* triangles = new Triangle[convex_hull->GetNumberOfCells()];

    double p0[3];
    double p1[3];
    double p2[3];

    vtkSmartPointer<vtkPoints> pts = vtkSmartPointer<vtkPoints>::New();
    for (size_t i = 0; i < convex_hull->GetNumberOfCells(); ++i) {
    vtkCell* cell = convex_hull->GetCell(i);
    vtkTriangle* triangle = dynamic_cast<vtkTriangle*>(cell);
    pts = cell->GetPoints();
    CHECK(pts->GetNumberOfPoints() == 3);

    triangle->GetPoints()->GetPoint(0, p0);
    triangle->GetPoints()->GetPoint(1, p1);
    triangle->GetPoints()->GetPoint(2, p2);

    triangles[i].x1 = p0[0];
    triangles[i].y1 = p0[1];
    triangles[i].z1 = p0[2];

    triangles[i].x2 = p1[0];
    triangles[i].y2 = p1[1];
    triangles[i].z2 = p1[2];

    triangles[i].x3 = p2[0];
    triangles[i].y3 = p2[1];
    triangles[i].z3 = p2[2];
    }

    *totalVolume = 0;
    double currentVolume;
    double xCenter = 0, yCenter = 0, zCenter = 0;

    for (int i = 0; i < convex_hull->GetNumberOfCells(); i++) {
    *totalVolume += currentVolume =
    (triangles[i].x1 * triangles[i].y2 * triangles[i].z3 -
    triangles[i].x1 * triangles[i].y3 * triangles[i].z2 -
    triangles[i].x2 * triangles[i].y1 * triangles[i].z3 +
    triangles[i].x2 * triangles[i].y3 * triangles[i].z1 +
    triangles[i].x3 * triangles[i].y1 * triangles[i].z2 -
    triangles[i].x3 * triangles[i].y2 * triangles[i].z1) /
    6;
    xCenter += ((triangles[i].x1 + triangles[i].x2 + triangles[i].x3) / 4) *
    currentVolume;
    yCenter += ((triangles[i].y1 + triangles[i].y2 + triangles[i].y3) / 4) *
    currentVolume;
    zCenter += ((triangles[i].z1 + triangles[i].z2 + triangles[i].z3) / 4) *
    currentVolume;
    }

    LOG(INFO) << "Total Volume = " << *totalVolume << " m³, are "
    << (*totalVolume) * 1000.0 << " liter.";
    LOG(INFO) << "X center = " << xCenter / *totalVolume;
    LOG(INFO) << "Y center = " << yCenter / *totalVolume;
    LOG(INFO) << "Z center = " << zCenter / *totalVolume;

    center_of_mass->x() = xCenter / *totalVolume;
    center_of_mass->y() = yCenter / *totalVolume;
    center_of_mass->z() = zCenter / *totalVolume;
    }