From 9ac5bf6b4bd844613973f078cbd2dc3669182236 Mon Sep 17 00:00:00 2001
From: jared-outotec <jared.le.cras@outotec.com>
Date: Fri, 29 Aug 2014 12:06:25 +0800
Subject: [PATCH] Fix bugs in cone visualization

---
 .../include/pcl/visualization/common/shapes.h | 22 ++++++++++++++++++-
 visualization/src/common/shapes.cpp           | 10 +++++----
 2 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/visualization/include/pcl/visualization/common/shapes.h b/visualization/include/pcl/visualization/common/shapes.h
index 854f980df59..ef36a0fe8de 100644
--- a/visualization/include/pcl/visualization/common/shapes.h
+++ b/visualization/include/pcl/visualization/common/shapes.h
@@ -222,7 +222,27 @@ namespace pcl
     create2DCircle (const pcl::ModelCoefficients &coefficients, double z = 0.0);
 
     /** \brief Create a cone shape from a set of model coefficients.
-      * \param[in] coefficients the cone coefficients (point_on_axis, axis_direction, radius)
+      * \param[in] coefficients the cone coefficients (cone_apex, axis_direction, angle)
+      *
+      * \code
+      * // The following are given (or computed using sample consensus techniques -- see SampleConsensusModelCone)
+      * // Eigen::Vector3f cone_apex, axis_direction;
+      * // float angle;
+      * // Note: The height of the cone is set using the magnitude of the axis_direction vector.
+      *
+      * pcl::ModelCoefficients cone_coeff;
+      * plane_coeff.values.resize (7);    // We need 7 values
+      * plane_coeff.values[0] = cone_apex.x ();
+      * plane_coeff.values[1] = cone_apex.y ();
+      * plane_coeff.values[2] = cone_apex.z ();
+      * plane_coeff.values[3] = axis_direction.x ();
+      * plane_coeff.values[4] = axis_direction.y ();
+      * plane_coeff.values[5] = axis_direction.z ();
+      * plane_coeff.values[6] = angle (); // degrees
+      *
+      * vtkSmartPointer<vtkDataSet> data = pcl::visualization::createCone (cone_coeff);
+      * \endcode
+      *
       * \ingroup visualization
       */
     PCL_EXPORTS vtkSmartPointer<vtkDataSet> 
diff --git a/visualization/src/common/shapes.cpp b/visualization/src/common/shapes.cpp
index 3fd89d5a0d6..7f2d7b182c7 100644
--- a/visualization/src/common/shapes.cpp
+++ b/visualization/src/common/shapes.cpp
@@ -249,10 +249,12 @@ vtkSmartPointer<vtkDataSet>
 pcl::visualization::createCone (const pcl::ModelCoefficients &coefficients)
 {
   vtkSmartPointer<vtkConeSource> cone = vtkSmartPointer<vtkConeSource>::New ();
-  cone->SetHeight (1.0);
-  cone->SetCenter (coefficients.values[0] + coefficients.values[3] * 0.5, 
-                   coefficients.values[1] + coefficients.values[1] * 0.5,
-                   coefficients.values[2] + coefficients.values[2] * 0.5);
+  cone->SetHeight (std::sqrt (coefficients.values[3] * coefficients.values[3] +
+                              coefficients.values[4] * coefficients.values[4] +
+                              coefficients.values[5] * coefficients.values[5]));
+  cone->SetCenter (coefficients.values[0] + coefficients.values[3] * 0.5,
+                   coefficients.values[1] + coefficients.values[4] * 0.5,
+                   coefficients.values[2] + coefficients.values[5] * 0.5);
   cone->SetDirection (-coefficients.values[3], -coefficients.values[4], -coefficients.values[5]);
   cone->SetResolution (100);
   cone->SetAngle (coefficients.values[6]);