diff --git a/GraspPlanning/CMakeLists.txt b/GraspPlanning/CMakeLists.txt
index 7017369bc2fe1e659d6df660d6f677687336fb69..a8b50bee21b60835552b10e7a83d3d95a7fb8bf4 100644
--- a/GraspPlanning/CMakeLists.txt
+++ b/GraspPlanning/CMakeLists.txt
@@ -57,6 +57,7 @@ GraspPlanner/GenericGraspPlanner.cpp
GraspQuality/GraspEvaluationPoseUncertainty.cpp
GraspQuality/GraspQualityMeasure.cpp
GraspQuality/GraspQualityMeasureWrenchSpace.cpp
+GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.cpp
Visualization/ConvexHullVisualization.cpp
)
@@ -73,6 +74,7 @@ GraspPlanner/GraspPlannerEvaluation.h
GraspQuality/GraspEvaluationPoseUncertainty.h
GraspQuality/GraspQualityMeasure.h
GraspQuality/GraspQualityMeasureWrenchSpace.h
+GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.h
Visualization/ConvexHullVisualization.h
)
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.cpp b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..436b632d7f5f95f851081d068c8abc0fc72fd127
--- /dev/null
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.cpp
@@ -0,0 +1,362 @@
+// **************************************************************
+// Implementation of class GraspQualityMeasure
+// **************************************************************
+// Author: Niko Vahrenkamp
+// Date: 26.10.2011
+// **************************************************************
+
+
+// **************************************************************
+// includes
+// **************************************************************
+
+#include "GraspQualityMeasureWrenchSpaceNotNormalized.h"
+#include <cmath>
+#include <cstdio>
+#include <cassert>
+#include <iostream>
+#include <sstream>
+#include <fstream>
+#include <string>
+#include <float.h>
+
+
+using namespace std;
+using namespace VirtualRobot;
+
+namespace GraspStudio
+{
+
+ void GraspQualityMeasureWrenchSpaceNotNormalized::setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint>& contactPoints)
+ {
+ GraspQualityMeasure::setContactPoints(contactPoints);
+ GWSCalculated = false;
+ }
+
+ void GraspQualityMeasureWrenchSpaceNotNormalized::setContactPoints(const VirtualRobot::EndEffector::ContactInfoVector& contactPoints)
+ {
+ GraspQualityMeasure::setContactPoints(contactPoints);
+ GWSCalculated = false;
+ }
+
+
+ float GraspQualityMeasureWrenchSpaceNotNormalized::getGraspQuality()
+ {
+ calculateGraspQuality();
+ return graspQuality;
+ }
+
+ bool GraspQualityMeasureWrenchSpaceNotNormalized::isGraspForceClosure()
+ {
+ updateGWS();
+ return isOriginInGWSHull();
+ }
+
+ void GraspQualityMeasureWrenchSpaceNotNormalized::updateGWS()
+ {
+ if (GWSCalculated)
+ {
+ return;
+ }
+ bool printAll = false;
+
+ if (contactPointsM.empty())
+ {
+ if (verbose && printAll)
+ printf("Contact points not set.\n");
+ return;
+ }
+
+ //Rotate generic friction cone to align with object normals
+ std::vector<VirtualRobot::MathTools::ContactPoint>::iterator objPointsIter;
+ std::vector<VirtualRobot::MathTools::ContactPoint> conePoints;
+
+ if (verbose && printAll)
+ {
+ cout << "GWS contact points:" << endl;
+ }
+
+ for (objPointsIter = contactPointsM.begin(); objPointsIter != contactPointsM.end(); objPointsIter++)
+ {
+ if (verbose && printAll)
+ {
+ MathTools::print((*objPointsIter));
+ }
+
+ coneGenerator->computeConePoints((*objPointsIter), conePoints);
+ }
+
+ //Generate convex hull from rotated contact friction cones
+ convexHullGWS = calculateConvexHull(conePoints);
+ //calculateHullCenter(convexHullGWS, convexHullCenterGWS);
+ convexHullCenterGWS = convexHullGWS->center;
+
+ if (verbose && printAll)
+ {
+ GRASPSTUDIO_INFO << " CENTER of GWS: " << endl;
+ MathTools::print(convexHullCenterGWS);
+ }
+
+ GWSCalculated = true;
+ }
+
+ VirtualRobot::MathTools::ConvexHull6DPtr GraspQualityMeasureWrenchSpaceNotNormalized::calculateConvexHull(std::vector<VirtualRobot::MathTools::ContactPoint>& points)
+ {
+ bool printAll = true;
+
+ if (verbose && printAll)
+ {
+ cout << "Convex hull points for wrench calculation:" << endl;
+ printContacts(points);
+ }
+
+ // create wrench
+ std::vector<VirtualRobot::MathTools::ContactPoint> wrenchPoints = createWrenchPoints(points, Eigen::Vector3f::Zero(), objectLength); // contact points are already moved so that com is at origin
+
+ if (verbose && printAll)
+ {
+ cout << "Wrench points:" << endl;
+ printContacts(wrenchPoints);
+ }
+
+ return ConvexHullGenerator::CreateConvexHull(wrenchPoints);
+ }
+
+ std::vector<VirtualRobot::MathTools::ContactPoint> GraspQualityMeasureWrenchSpaceNotNormalized::createWrenchPoints(std::vector<VirtualRobot::MathTools::ContactPoint>& points, const Eigen::Vector3f& centerOfModel, float objectLengthMM)
+ {
+ std::vector<VirtualRobot::MathTools::ContactPoint> result;
+ std::vector<VirtualRobot::MathTools::ContactPoint>::iterator iter = points.begin();
+ VirtualRobot::MathTools::ContactPoint p;
+ Eigen::Vector3f normal;
+
+ // convert objLength from mm to m
+ bool convertMM2M = true;
+
+ float factor = 1.0f;
+
+ if (objectLengthMM != 0)
+ {
+ if (convertMM2M)
+ {
+ factor = 2.0f / (objectLengthMM / 1000.0f); // == max distance from center ( 1 / (length/2) )
+ }
+ else
+ {
+ factor = 2.0f / objectLengthMM; // == max distance from center ( 1 / (length/2) )
+ }
+ }
+
+ VirtualRobot::MathTools::ContactPoint tmpP;
+
+ while (iter != points.end())
+ {
+#ifdef INVERT_NORMALS
+ /*p.p(0) = -(*iter).n(0);
+ p.p(1) = -(*iter).n(1);
+ p.p(2) = -(*iter).n(2);*/
+ p.p = -1.0f * (iter->n);
+#else
+ p.p = iter->n;
+#endif
+ tmpP.p = iter->p - centerOfModel;
+ tmpP.n = -(iter->n);
+
+ //normal = crossProductPosNormalInv(tmpP);
+ //p.n = factor * normal;
+ p.n = factor * tmpP.p.cross(tmpP.n);
+
+ result.push_back(p);
+ iter++;
+ }
+
+ return result;
+ }
+
+ void GraspQualityMeasureWrenchSpaceNotNormalized::printContacts(std::vector<VirtualRobot::MathTools::ContactPoint>& points)
+ {
+ std::vector<VirtualRobot::MathTools::ContactPoint>::iterator iter = points.begin();
+
+ while (iter != points.end())
+ {
+ cout << "# ";
+ MathTools::print((*iter));
+ iter++;
+ }
+ }
+
+ VirtualRobot::MathTools::ContactPoint GraspQualityMeasureWrenchSpaceNotNormalized::calculateHullCenter(VirtualRobot::MathTools::ConvexHull6DPtr hull)
+ {
+ if (!hull)
+ {
+ GRASPSTUDIO_ERROR << "NULL data?!" << endl;
+ return VirtualRobot::MathTools::ContactPoint();
+ }
+
+ VirtualRobot::MathTools::ContactPoint resultCenter;
+ std::vector<VirtualRobot::MathTools::ContactPoint>::iterator iter;
+ resultCenter.p.setZero();
+ resultCenter.n.setZero();
+
+ if (hull->vertices.size() == 0)
+ {
+ cout << __FUNCTION__ << ": error, no vertices..." << endl;
+ return resultCenter;
+ }
+
+ for (iter = hull->vertices.begin(); iter != hull->vertices.end(); iter++)
+ {
+ resultCenter.p += iter->p;
+ resultCenter.n += iter->n;
+ }
+
+ resultCenter.p /= (float)hull->vertices.size();
+ resultCenter.n /= (float)hull->vertices.size();
+ return resultCenter;
+ }
+
+ float GraspQualityMeasureWrenchSpaceNotNormalized::minDistanceToGWSHull(VirtualRobot::MathTools::ContactPoint& point)
+ {
+ float minDist = FLT_MAX;
+ float dist[6];
+ float currentDist2;
+ std::vector<MathTools::TriangleFace6D>::iterator faceIter;
+
+ for (faceIter = convexHullGWS->faces.begin(); faceIter != convexHullGWS->faces.end(); faceIter++)
+ {
+ VirtualRobot::MathTools::ContactPoint faceCenter;
+ faceCenter.p.setZero();
+ faceCenter.n.setZero();
+
+ for (int j = 0; j < 6; j++)
+ {
+ faceCenter.p += (convexHullGWS->vertices)[faceIter->id[j]].p;
+ faceCenter.n += (convexHullGWS->vertices)[faceIter->id[j]].n;
+ }
+
+ faceCenter.p /= 6.0f;
+ faceCenter.n /= 6.0f;
+
+ currentDist2 = 0;
+
+ for (int j = 0; j < 3; j++)
+ {
+ dist[j] = (faceCenter.p(j) - point.p(j));
+ dist[j + 3] = (faceCenter.n(j) - point.n(j));
+ currentDist2 += dist[j] * dist[j];
+ currentDist2 += dist[j + 3] * dist[j + 3];
+ }
+
+ if (currentDist2 < minDist)
+ {
+ minDist = currentDist2;
+ }
+ }
+
+ return sqrtf(minDist);
+ }
+ bool GraspQualityMeasureWrenchSpaceNotNormalized::isOriginInGWSHull()
+ {
+ if (!GWSCalculated || !convexHullGWS)
+ {
+ return false;
+ }
+
+ for (const auto& face : convexHullGWS->faces)
+ {
+ // ignore rounding errors
+ if (face.distPlaneZero > 1e-4)
+ {
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+
+
+ float GraspQualityMeasureWrenchSpaceNotNormalized::minOffset(VirtualRobot::MathTools::ConvexHull6DPtr ch)
+ {
+ if (!ch)
+ {
+ return 0.0f;
+ }
+
+ float fRes = FLT_MAX;
+ int nWrongFacets = 0;
+
+ for (size_t i = 0; i < ch->faces.size(); i++)
+ {
+ const auto dist = ch->faces[i].distNormCenter;
+ if (dist > 0)
+ {
+ //outside
+ nWrongFacets++;
+ continue;
+ }
+ fRes = std::max(fRes, -dist);
+ }
+
+ if (nWrongFacets > 0)
+ {
+ cout << __FUNCTION__ << " Warning: offset of " << nWrongFacets << " facets >0 (# of facets:" << ch->faces.size() << ")" << endl;
+ }
+
+ return fRes;
+ }
+
+ float GraspQualityMeasureWrenchSpaceNotNormalized::getVolumeGraspMeasure()
+ {
+ updateGWS();
+
+ if (!convexHullGWS || convexHullGWS->vertices.size() == 0)
+ {
+ cout << __FUNCTION__ << "No vertices in Grasp Wrench Space?! Maybe I was not initialized correctly..." << endl;
+ return 0.0;
+ }
+ return convexHullGWS->volume;
+ }
+
+ bool GraspQualityMeasureWrenchSpaceNotNormalized::calculateGraspQuality()
+ {
+ updateGWS();
+
+ graspQuality = 0.0f;
+
+ if (!convexHullGWS || convexHullGWS->vertices.size() == 0)
+ {
+ cout << __FUNCTION__ << "No vertices in Grasp Wrench Space?! Maybe I was not initialized correctly..." << endl;
+ return 0.0;
+ }
+
+ float fResOffsetGWS = minOffset(convexHullGWS);
+
+ graspQuality = fResOffsetGWS;
+
+ if (verbose)
+ {
+ GRASPSTUDIO_INFO << endl;
+ cout << ": GWS volume : " << convexHullGWS->volume << endl;
+ cout << ": GWS min Offset: " << fResOffsetGWS << endl;
+ cout << ": GraspQuality : " << graspQuality << endl;
+ }
+
+ return true;
+ }
+
+ std::string GraspQualityMeasureWrenchSpaceNotNormalized::getName()
+ {
+ return "GraspWrenchSpaceNotNormalized";
+ }
+
+
+ Eigen::Vector3f GraspQualityMeasureWrenchSpaceNotNormalized::crossProductPosNormalInv(const VirtualRobot::MathTools::ContactPoint& v1)
+ {
+ Eigen::Vector3f res;
+ res(0) = v1.p(1) * (-v1.n(2)) - v1.p(2) * (-v1.n(1));
+ res(1) = v1.p(2) * (-v1.n(0)) - v1.p(0) * (-v1.n(2));
+ res(2) = v1.p(0) * (-v1.n(1)) - v1.p(1) * (-v1.n(0));
+ return res;
+ }
+
+}
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.h b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.h
new file mode 100644
index 0000000000000000000000000000000000000000..a26b121c23a2fcbf494391fc0d74a494a0c611ce
--- /dev/null
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.h
@@ -0,0 +1,128 @@
+/**
+* This file is part of Simox.
+*
+* Simox 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; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox 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 program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Nikolaus Vahrenkamp
+* @copyright 2011 Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#pragma once
+
+#include "../GraspStudio.h"
+#include "GraspQualityMeasure.h"
+
+#include <Eigen/Core>
+
+namespace GraspStudio
+{
+
+ /*!
+ \brief An efficient implementation of the grasp wrench space algorithm for grasp quality evaluation.
+
+ the grasp wrench space algorithm is widely used in the context of grasp planning. By analyzing
+ the grasp wrench space (GWS) of a given set of contact points, a quality score of a grasp
+ can be evaluated.
+ In this implementation, additionally an object specific wrench space (WS) is calculated, which
+ approximatevly represents a "perfect" grasp. This object is used to normalize the quality score.
+ */
+ class GRASPSTUDIO_IMPORT_EXPORT GraspQualityMeasureWrenchSpaceNotNormalized : public GraspQualityMeasure
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ //inherit ctor
+ using GraspQualityMeasure::GraspQualityMeasure;
+
+
+ //! This method is used to compute a reference value that describes a perfect grasp
+ bool calculateObjectProperties() override { return true; }
+
+ /*!
+ Returns f_max_gws
+ with f_max_gws = max distance of GWS hull center to one of its facets
+ -> also known as "epsilon" quality == radius of larges enclosing 6D ball
+ */
+ float getGraspQuality() override;
+
+ /*!
+ Volume grasp quality ratio of GWS volume
+ -> also known as "v" quality
+ */
+ virtual float getVolumeGraspMeasure();
+
+ /*
+ Checks if wrench space origin is inside GWS-Hull
+ */
+ bool isGraspForceClosure() override;
+
+ /*
+ Returns the internally calculated convex hull object (GraspWrenchSpace)
+ This hull depends on the contacts
+ */
+ virtual VirtualRobot::MathTools::ConvexHull6DPtr getConvexHullGWS()
+ {
+ return convexHullGWS;
+ }
+
+ void updateGWS();
+
+ VirtualRobot::MathTools::ContactPoint getCenterGWS()
+ {
+ return convexHullCenterGWS;
+ }
+
+ /*!
+ setup contact information
+ the contact points are normalized by subtracting the COM
+ the contact normals are normalize to unit length
+ */
+ void setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint>& contactPoints) override;
+ void setContactPoints(const VirtualRobot::EndEffector::ContactInfoVector& contactPoints) override;
+
+ bool calculateGraspQuality() override;
+
+ //! Returns description of this object
+ std::string getName() override;
+
+
+ static std::vector<VirtualRobot::MathTools::ContactPoint> createWrenchPoints(std::vector<VirtualRobot::MathTools::ContactPoint>& points, const Eigen::Vector3f& centerOfModel, float objectLengthMM);
+
+ //! Goes through all facets of convex hull and searches the minimum distance to it's center
+ static float minOffset(VirtualRobot::MathTools::ConvexHull6DPtr ch);
+
+ protected:
+
+ //Methods
+ VirtualRobot::MathTools::ConvexHull6DPtr calculateConvexHull(std::vector<VirtualRobot::MathTools::ContactPoint>& points);
+ VirtualRobot::MathTools::ContactPoint calculateHullCenter(VirtualRobot::MathTools::ConvexHull6DPtr hull);
+
+ float minDistanceToGWSHull(VirtualRobot::MathTools::ContactPoint& point);
+
+
+ bool isOriginInGWSHull();
+ void printContacts(std::vector<VirtualRobot::MathTools::ContactPoint>& points);
+ static Eigen::Vector3f crossProductPosNormalInv(const VirtualRobot::MathTools::ContactPoint& v1);
+
+ //For safety
+ bool GWSCalculated {false};
+
+ //For Object and Grasp Wrench Space Calculation
+ VirtualRobot::MathTools::ConvexHull6DPtr convexHullGWS;
+ VirtualRobot::MathTools::ContactPoint convexHullCenterGWS;
+ };
+}
+
diff --git a/GraspPlanning/GraspStudio.h b/GraspPlanning/GraspStudio.h
index 7cf38174fae824250ef60703bd8e1f1f5a169f5e..1d6de55a64e0a115dca56580d1101a5134e97f02 100644
--- a/GraspPlanning/GraspStudio.h
+++ b/GraspPlanning/GraspStudio.h
@@ -73,6 +73,7 @@ namespace GraspStudio
class GraspQualityMeasure;
class GraspQualityMeasureWrenchSpace;
+ class GraspQualityMeasureWrenchSpaceNotNormalized;
class ContactConeGenerator;
class GraspQualityMeasure;
@@ -83,6 +84,7 @@ namespace GraspStudio
typedef boost::shared_ptr<GraspQualityMeasure> GraspQualityMeasurePtr;
typedef boost::shared_ptr<GraspQualityMeasureWrenchSpace> GraspQualityMeasureWrenchSpacePtr;
+ typedef boost::shared_ptr<GraspQualityMeasureWrenchSpaceNotNormalized> GraspQualityMeasureWrenchSpaceNotNormalizedPtr;
typedef boost::shared_ptr<ContactConeGenerator> ContactConeGeneratorPtr;
typedef boost::shared_ptr<GraspQualityMeasure> GraspQualityMeasurePtr;
typedef boost::shared_ptr<ApproachMovementGenerator> ApproachMovementGeneratorPtr;
diff --git a/VirtualRobot/CollisionDetection/PQP/PQP++/BV.cpp b/VirtualRobot/CollisionDetection/PQP/PQP++/BV.cpp
index 9ad3ea6136e5c309faa8454353d80643dc6e7194..31d7ecc447120ffbbbadf27a4f923358e3532c11 100644
--- a/VirtualRobot/CollisionDetection/PQP/PQP++/BV.cpp
+++ b/VirtualRobot/CollisionDetection/PQP/PQP++/BV.cpp
@@ -38,37 +38,19 @@
\**************************************************************************/
+#include <algorithm>
+
#include <stdlib.h>
#include <math.h>
+
#include "BV.h"
#include "MatVec.h"
#include "RectDist.h"
#include "OBB_Disjoint.h"
+
namespace PQP
{
-
- BV::BV()
- {
- first_child = 0;
- }
-
- BV::~BV()
- {
- }
-
- inline
- PQP_REAL
- BV::MaxOfTwo(PQP_REAL a, PQP_REAL b)
- {
- if (a > b)
- {
- return a;
- }
-
- return b;
- }
-
void
BV::FitToTris(PQP_REAL O[3][3], Tri* tris, int num_tris)
{
@@ -95,286 +77,272 @@ namespace PQP
point++;
}
- PQP_REAL minx, maxx, miny, maxy, minz, maxz, c[3];
#if PQP_BV_TYPE & OBB_TYPE
- minx = maxx = P[0][0];
- miny = maxy = P[0][1];
- minz = maxz = P[0][2];
-
- for (i = 1; i < num_points; i++)
{
- if (P[i][0] < minx)
- {
- minx = P[i][0];
- }
- else if (P[i][0] > maxx)
- {
- maxx = P[i][0];
- }
+ PQP_REAL minx, maxx, miny, maxy, minz, maxz, c[3];
+ minx = maxx = P[0][0];
+ miny = maxy = P[0][1];
+ minz = maxz = P[0][2];
- if (P[i][1] < miny)
+ for (i = 1; i < num_points; i++)
{
- miny = P[i][1];
- }
- else if (P[i][1] > maxy)
- {
- maxy = P[i][1];
- }
+ if (P[i][0] < minx)
+ {
+ minx = P[i][0];
+ }
+ else if (P[i][0] > maxx)
+ {
+ maxx = P[i][0];
+ }
- if (P[i][2] < minz)
- {
- minz = P[i][2];
- }
- else if (P[i][2] > maxz)
- {
- maxz = P[i][2];
+ if (P[i][1] < miny)
+ {
+ miny = P[i][1];
+ }
+ else if (P[i][1] > maxy)
+ {
+ maxy = P[i][1];
+ }
+
+ if (P[i][2] < minz)
+ {
+ minz = P[i][2];
+ }
+ else if (P[i][2] > maxz)
+ {
+ maxz = P[i][2];
+ }
}
- }
- c[0] = (PQP_REAL)0.5 * (maxx + minx);
- c[1] = (PQP_REAL)0.5 * (maxy + miny);
- c[2] = (PQP_REAL)0.5 * (maxz + minz);
- pqp_math.MxV(To, R, c);
+ c[0] = (PQP_REAL)0.5 * (maxx + minx);
+ c[1] = (PQP_REAL)0.5 * (maxy + miny);
+ c[2] = (PQP_REAL)0.5 * (maxz + minz);
+ pqp_math.MxV(To, R, c);
- d[0] = (PQP_REAL)0.5 * (maxx - minx);
- d[1] = (PQP_REAL)0.5 * (maxy - miny);
- d[2] = (PQP_REAL)0.5 * (maxz - minz);
+ d[0] = (PQP_REAL)0.5 * (maxx - minx);
+ d[1] = (PQP_REAL)0.5 * (maxy - miny);
+ d[2] = (PQP_REAL)0.5 * (maxz - minz);
+ }
#endif
#if PQP_BV_TYPE & RSS_TYPE
-
- // compute thickness, which determines radius, and z of rectangle corner
-
- PQP_REAL cz, radsqr;
- minz = maxz = P[0][2];
-
- for (i = 1; i < num_points; i++)
{
- if (P[i][2] < minz)
- {
- minz = P[i][2];
- }
- else if (P[i][2] > maxz)
- {
- maxz = P[i][2];
- }
- }
-
- r = (PQP_REAL)0.5 * (maxz - minz);
- radsqr = r * r;
- cz = (PQP_REAL)0.5 * (maxz + minz);
+ PQP_REAL minx, maxx, miny, maxy, c[3];
+ PQP_REAL cz, radsqr;
- // compute an initial length of rectangle along x direction
+ // compute thickness, which determines radius, and z of rectangle corner
+ //AND
+ // compute an initial length of rectangle along x / y direction
+ // find minx and maxx / miny and maxy as starting points
- // find minx and maxx as starting points
-
- int minindex, maxindex;
- minindex = maxindex = 0;
-
- for (i = 1; i < num_points; i++)
- {
- if (P[i][0] < P[minindex][0])
{
- minindex = i;
- }
- else if (P[i][0] > P[maxindex][0])
- {
- maxindex = i;
- }
- }
+ int x_minindex = 0;
+ int x_maxindex = 0;
+ int y_minindex = 0;
+ int y_maxindex = 0;
- PQP_REAL x, dz;
- dz = P[minindex][2] - cz;
- minx = P[minindex][0] + sqrt(MaxOfTwo(radsqr - dz * dz, 0));
- dz = P[maxindex][2] - cz;
- maxx = P[maxindex][0] - sqrt(MaxOfTwo(radsqr - dz * dz, 0));
-
- // grow minx
-
- for (i = 0; i < num_points; i++)
- {
- if (P[i][0] < minx)
- {
- dz = P[i][2] - cz;
- x = P[i][0] + sqrt(MaxOfTwo(radsqr - dz * dz, 0));
-
- if (x < minx)
{
- minx = x;
- }
- }
- }
+ PQP_REAL minz = P[0][2];
+ PQP_REAL maxz = P[0][2];
- // grow maxx
-
- for (i = 0; i < num_points; i++)
- {
- if (P[i][0] > maxx)
- {
- dz = P[i][2] - cz;
- x = P[i][0] - sqrt(MaxOfTwo(radsqr - dz * dz, 0));
-
- if (x > maxx)
- {
- maxx = x;
+ for (i = 1; i < num_points; i++)
+ {
+ if (P[i][0] < P[x_minindex][0])
+ {
+ x_minindex = i;
+ }
+ else if (P[i][0] > P[x_maxindex][0])
+ {
+ x_maxindex = i;
+ }
+
+ if (P[i][1] < P[y_minindex][1])
+ {
+ y_minindex = i;
+ }
+ else if (P[i][1] > P[y_maxindex][1])
+ {
+ y_maxindex = i;
+ }
+
+ if (P[i][2] < minz)
+ {
+ minz = P[i][2];
+ }
+ else if (P[i][2] > maxz)
+ {
+ maxz = P[i][2];
+ }
+ }
+ r = (PQP_REAL)0.5 * (maxz - minz);
+ radsqr = r * r;
+ cz = (PQP_REAL)0.5 * (maxz + minz);
}
- }
- }
-
- // compute an initial length of rectangle along y direction
-
- // find miny and maxy as starting points
- minindex = maxindex = 0;
+ PQP_REAL dz;
+ dz = P[x_minindex][2] - cz;
+ minx = P[x_minindex][0] + sqrt(std::max(radsqr - dz * dz, 0.f));
+ dz = P[x_maxindex][2] - cz;
+ maxx = P[x_maxindex][0] - sqrt(std::max(radsqr - dz * dz, 0.f));
- for (i = 1; i < num_points; i++)
- {
- if (P[i][1] < P[minindex][1])
- {
- minindex = i;
+ dz = P[y_minindex][2] - cz;
+ miny = P[y_minindex][1] + sqrt(std::max(radsqr - dz * dz, 0.f));
+ dz = P[y_maxindex][2] - cz;
+ maxy = P[y_maxindex][1] - sqrt(std::max(radsqr - dz * dz, 0.f));
}
- else if (P[i][1] > P[maxindex][1])
+
+ // grow minx / maxx / miny / maxy
{
- maxindex = i;
- }
- }
+ for (i = 0; i < num_points; i++)
+ {
+ // grow minx
+ if (P[i][0] < minx)
+ {
+ const PQP_REAL dz = P[i][2] - cz;
+ const PQP_REAL x = P[i][0] + sqrt(std::max(radsqr - dz * dz, 0.f));
- PQP_REAL y;
- dz = P[minindex][2] - cz;
- miny = P[minindex][1] + sqrt(MaxOfTwo(radsqr - dz * dz, 0));
- dz = P[maxindex][2] - cz;
- maxy = P[maxindex][1] - sqrt(MaxOfTwo(radsqr - dz * dz, 0));
+ if (x < minx)
+ {
+ minx = x;
+ }
+ }
- // grow miny
+ // grow maxx
+ if (P[i][0] > maxx)
+ {
+ const PQP_REAL dz = P[i][2] - cz;
+ const PQP_REAL x = P[i][0] - sqrt(std::max(radsqr - dz * dz, 0.f));
- for (i = 0; i < num_points; i++)
- {
- if (P[i][1] < miny)
- {
- dz = P[i][2] - cz;
- y = P[i][1] + sqrt(MaxOfTwo(radsqr - dz * dz, 0));
+ if (x > maxx)
+ {
+ maxx = x;
+ }
+ }
- if (y < miny)
- {
- miny = y;
- }
- }
- }
+ // grow miny
+ if (P[i][1] < miny)
+ {
+ const PQP_REAL dz = P[i][2] - cz;
+ const PQP_REAL y = P[i][1] + sqrt(std::max(radsqr - dz * dz, 0.f));
- // grow maxy
+ if (y < miny)
+ {
+ miny = y;
+ }
+ }
- for (i = 0; i < num_points; i++)
- {
- if (P[i][1] > maxy)
- {
- dz = P[i][2] - cz;
- y = P[i][1] - sqrt(MaxOfTwo(radsqr - dz * dz, 0));
+ // grow maxy
+ if (P[i][1] > maxy)
+ {
+ const PQP_REAL dz = P[i][2] - cz;
+ const PQP_REAL y = P[i][1] - sqrt(std::max(radsqr - dz * dz, 0.f));
- if (y > maxy)
- {
- maxy = y;
+ if (y > maxy)
+ {
+ maxy = y;
+ }
+ }
}
}
- }
- // corners may have some points which are not covered - grow lengths if
- // necessary
+ // corners may have some points which are not covered - grow lengths if
+ // necessary
- PQP_REAL dx, dy, u, t;
- PQP_REAL a = sqrt((PQP_REAL)0.5);
+ PQP_REAL a = sqrt((PQP_REAL)0.5);
- for (i = 0; i < num_points; i++)
- {
- if (P[i][0] > maxx)
+ for (i = 0; i < num_points; i++)
{
- if (P[i][1] > maxy)
+ if (P[i][0] > maxx)
{
- dx = P[i][0] - maxx;
- dy = P[i][1] - maxy;
- u = dx * a + dy * a;
- t = (a * u - dx) * (a * u - dx) +
- (a * u - dy) * (a * u - dy) +
- (cz - P[i][2]) * (cz - P[i][2]);
- u = u - sqrt(MaxOfTwo(radsqr - t, 0));
-
- if (u > 0)
+ if (P[i][1] > maxy)
{
- maxx += u * a;
- maxy += u * a;
+ const PQP_REAL dx = P[i][0] - maxx;
+ const PQP_REAL dy = P[i][1] - maxy;
+ PQP_REAL u = dx * a + dy * a;
+ const PQP_REAL t = (a * u - dx) * (a * u - dx) +
+ (a * u - dy) * (a * u - dy) +
+ (cz - P[i][2]) * (cz - P[i][2]);
+ u = u - sqrt(std::max(radsqr - t, 0.f));
+
+ if (u > 0)
+ {
+ maxx += u * a;
+ maxy += u * a;
+ }
}
- }
- else if (P[i][1] < miny)
- {
- dx = P[i][0] - maxx;
- dy = P[i][1] - miny;
- u = dx * a - dy * a;
- t = (a * u - dx) * (a * u - dx) +
- (-a * u - dy) * (-a * u - dy) +
- (cz - P[i][2]) * (cz - P[i][2]);
- u = u - sqrt(MaxOfTwo(radsqr - t, 0));
-
- if (u > 0)
+ else if (P[i][1] < miny)
{
- maxx += u * a;
- miny -= u * a;
+ const PQP_REAL dx = P[i][0] - maxx;
+ const PQP_REAL dy = P[i][1] - miny;
+ PQP_REAL u = dx * a - dy * a;
+ const PQP_REAL t = (a * u - dx) * (a * u - dx) +
+ (-a * u - dy) * (-a * u - dy) +
+ (cz - P[i][2]) * (cz - P[i][2]);
+ u = u - sqrt(std::max(radsqr - t, 0.f));
+
+ if (u > 0)
+ {
+ maxx += u * a;
+ miny -= u * a;
+ }
}
}
- }
- else if (P[i][0] < minx)
- {
- if (P[i][1] > maxy)
+ else if (P[i][0] < minx)
{
- dx = P[i][0] - minx;
- dy = P[i][1] - maxy;
- u = dy * a - dx * a;
- t = (-a * u - dx) * (-a * u - dx) +
- (a * u - dy) * (a * u - dy) +
- (cz - P[i][2]) * (cz - P[i][2]);
- u = u - sqrt(MaxOfTwo(radsqr - t, 0));
-
- if (u > 0)
+ if (P[i][1] > maxy)
{
- minx -= u * a;
- maxy += u * a;
+ const PQP_REAL dx = P[i][0] - minx;
+ const PQP_REAL dy = P[i][1] - maxy;
+ PQP_REAL u = dy * a - dx * a;
+ const PQP_REAL t = (-a * u - dx) * (-a * u - dx) +
+ (a * u - dy) * (a * u - dy) +
+ (cz - P[i][2]) * (cz - P[i][2]);
+ u = u - sqrt(std::max(radsqr - t, 0.f));
+
+ if (u > 0)
+ {
+ minx -= u * a;
+ maxy += u * a;
+ }
}
- }
- else if (P[i][1] < miny)
- {
- dx = P[i][0] - minx;
- dy = P[i][1] - miny;
- u = -dx * a - dy * a;
- t = (-a * u - dx) * (-a * u - dx) +
- (-a * u - dy) * (-a * u - dy) +
- (cz - P[i][2]) * (cz - P[i][2]);
- u = u - sqrt(MaxOfTwo(radsqr - t, 0));
-
- if (u > 0)
+ else if (P[i][1] < miny)
{
- minx -= u * a;
- miny -= u * a;
+ const PQP_REAL dx = P[i][0] - minx;
+ const PQP_REAL dy = P[i][1] - miny;
+ PQP_REAL u = -dx * a - dy * a;
+ const PQP_REAL t = (-a * u - dx) * (-a * u - dx) +
+ (-a * u - dy) * (-a * u - dy) +
+ (cz - P[i][2]) * (cz - P[i][2]);
+ u = u - sqrt(std::max(radsqr - t, 0.f));
+
+ if (u > 0)
+ {
+ minx -= u * a;
+ miny -= u * a;
+ }
}
}
}
- }
- c[0] = minx;
- c[1] = miny;
- c[2] = cz;
- pqp_math.MxV(Tr, R, c);
+ c[0] = minx;
+ c[1] = miny;
+ c[2] = cz;
+ pqp_math.MxV(Tr, R, c);
- l[0] = maxx - minx;
+ l[0] = maxx - minx;
- if (l[0] < 0)
- {
- l[0] = 0;
- }
+ if (l[0] < 0)
+ {
+ l[0] = 0;
+ }
- l[1] = maxy - miny;
+ l[1] = maxy - miny;
- if (l[1] < 0)
- {
- l[1] = 0;
+ if (l[1] < 0)
+ {
+ l[1] = 0;
+ }
}
#endif
diff --git a/VirtualRobot/CollisionDetection/PQP/PQP++/BV.h b/VirtualRobot/CollisionDetection/PQP/PQP++/BV.h
index a05fcfdf12bfb1596e7a6b415e34f6587e866c9e..a5a39ff92d0d55db2fecf0356e52fa67c62cc659 100644
--- a/VirtualRobot/CollisionDetection/PQP/PQP++/BV.h
+++ b/VirtualRobot/CollisionDetection/PQP/PQP++/BV.h
@@ -65,11 +65,11 @@ namespace PQP
PQP_REAL d[3]; // (half) dimensions of obb
#endif
- int first_child; // positive value is index of first_child bv
+ int first_child{0}; // positive value is index of first_child bv
// negative value is -(index + 1) of triangle
- BV();
- ~BV();
+ BV() = default;
+ ~BV() = default;
int Leaf()
{
return first_child < 0;
@@ -78,8 +78,6 @@ namespace PQP
void FitToTris(PQP_REAL O[3][3], Tri* tris, int num_tris);
MatVec pqp_math;
- PQP_REAL MaxOfTwo(PQP_REAL a, PQP_REAL b);
-
};
inline
diff --git a/VirtualRobot/CollisionDetection/PQP/PQP++/OBB_Disjoint.h b/VirtualRobot/CollisionDetection/PQP/PQP++/OBB_Disjoint.h
index f37304f974beef0eda085d79d82c62333eb3fbc7..353c75ac63e6333187078a93faacc6753dd5bbda 100644
--- a/VirtualRobot/CollisionDetection/PQP/PQP++/OBB_Disjoint.h
+++ b/VirtualRobot/CollisionDetection/PQP/PQP++/OBB_Disjoint.h
@@ -40,6 +40,8 @@
#pragma once
+#include <cmath>
+
#include "MatVec.h"
#include "PQP_Compile.h"
@@ -66,38 +68,33 @@ namespace PQP
int
obb_disjoint(PQP_REAL B[3][3], PQP_REAL T[3], PQP_REAL a[3], PQP_REAL b[3])
{
- PQP_REAL t, s;
+ PQP_REAL s;
int r;
PQP_REAL Bf[3][3];
- const PQP_REAL reps = (PQP_REAL)1e-6;
-
- // Bf = fabs(B)
- Bf[0][0] = myfabs(B[0][0]);
- Bf[0][0] += reps;
- Bf[0][1] = myfabs(B[0][1]);
- Bf[0][1] += reps;
- Bf[0][2] = myfabs(B[0][2]);
- Bf[0][2] += reps;
- Bf[1][0] = myfabs(B[1][0]);
- Bf[1][0] += reps;
- Bf[1][1] = myfabs(B[1][1]);
- Bf[1][1] += reps;
- Bf[1][2] = myfabs(B[1][2]);
- Bf[1][2] += reps;
- Bf[2][0] = myfabs(B[2][0]);
- Bf[2][0] += reps;
- Bf[2][1] = myfabs(B[2][1]);
- Bf[2][1] += reps;
- Bf[2][2] = myfabs(B[2][2]);
- Bf[2][2] += reps;
+
+ {
+ static constexpr PQP_REAL reps = (PQP_REAL)1e-6;
+
+ // Bf = fabs(B)
+ Bf[0][0] = std::abs(B[0][0]) + reps;
+ Bf[0][1] = std::abs(B[0][1]) + reps;
+ Bf[0][2] = std::abs(B[0][2]) + reps;
+
+ Bf[1][0] = std::abs(B[1][0]) + reps;
+ Bf[1][1] = std::abs(B[1][1]) + reps;
+ Bf[1][2] = std::abs(B[1][2]) + reps;
+
+ Bf[2][0] = std::abs(B[2][0]) + reps;
+ Bf[2][1] = std::abs(B[2][1]) + reps;
+ Bf[2][2] = std::abs(B[2][2]) + reps;
+ }
// if any of these tests are one-sided, then the polyhedra are disjoint
r = 1;
// A1 x A2 = A0
- t = myfabs(T[0]);
- r &= (t <=
+ r &= (std::abs(T[0]) <=
(a[0] + b[0] * Bf[0][0] + b[1] * Bf[0][1] + b[2] * Bf[0][2]));
if (!r)
@@ -107,9 +104,8 @@ namespace PQP
// B1 x B2 = B0
s = T[0] * B[0][0] + T[1] * B[1][0] + T[2] * B[2][0];
- t = myfabs(s);
- r &= (t <=
+ r &= (std::abs(s) <=
(b[0] + a[0] * Bf[0][0] + a[1] * Bf[1][0] + a[2] * Bf[2][0]));
if (!r)
@@ -118,9 +114,8 @@ namespace PQP
}
// A2 x A0 = A1
- t = myfabs(T[1]);
- r &= (t <=
+ r &= (std::abs(T[1]) <=
(a[1] + b[0] * Bf[1][0] + b[1] * Bf[1][1] + b[2] * Bf[1][2]));
if (!r)
@@ -129,9 +124,8 @@ namespace PQP
}
// A0 x A1 = A2
- t = myfabs(T[2]);
- r &= (t <=
+ r &= (std::abs(T[2]) <=
(a[2] + b[0] * Bf[2][0] + b[1] * Bf[2][1] + b[2] * Bf[2][2]));
if (!r)
@@ -141,9 +135,8 @@ namespace PQP
// B2 x B0 = B1
s = T[0] * B[0][1] + T[1] * B[1][1] + T[2] * B[2][1];
- t = myfabs(s);
- r &= (t <=
+ r &= (std::abs(s) <=
(b[1] + a[0] * Bf[0][1] + a[1] * Bf[1][1] + a[2] * Bf[2][1]));
if (!r)
@@ -153,9 +146,8 @@ namespace PQP
// B0 x B1 = B2
s = T[0] * B[0][2] + T[1] * B[1][2] + T[2] * B[2][2];
- t = myfabs(s);
- r &= (t <=
+ r &= (std::abs(s) <=
(b[2] + a[0] * Bf[0][2] + a[1] * Bf[1][2] + a[2] * Bf[2][2]));
if (!r)
@@ -165,9 +157,8 @@ namespace PQP
// A0 x B0
s = T[2] * B[1][0] - T[1] * B[2][0];
- t = myfabs(s);
- r &= (t <=
+ r &= (std::abs(s) <=
(a[1] * Bf[2][0] + a[2] * Bf[1][0] +
b[1] * Bf[0][2] + b[2] * Bf[0][1]));
@@ -178,9 +169,8 @@ namespace PQP
// A0 x B1
s = T[2] * B[1][1] - T[1] * B[2][1];
- t = myfabs(s);
- r &= (t <=
+ r &= (std::abs(s) <=
(a[1] * Bf[2][1] + a[2] * Bf[1][1] +
b[0] * Bf[0][2] + b[2] * Bf[0][0]));
@@ -191,9 +181,8 @@ namespace PQP
// A0 x B2
s = T[2] * B[1][2] - T[1] * B[2][2];
- t = myfabs(s);
- r &= (t <=
+ r &= (std::abs(s) <=
(a[1] * Bf[2][2] + a[2] * Bf[1][2] +
b[0] * Bf[0][1] + b[1] * Bf[0][0]));
@@ -204,9 +193,8 @@ namespace PQP
// A1 x B0
s = T[0] * B[2][0] - T[2] * B[0][0];
- t = myfabs(s);
- r &= (t <=
+ r &= (std::abs(s) <=
(a[0] * Bf[2][0] + a[2] * Bf[0][0] +
b[1] * Bf[1][2] + b[2] * Bf[1][1]));
@@ -217,9 +205,8 @@ namespace PQP
// A1 x B1
s = T[0] * B[2][1] - T[2] * B[0][1];
- t = myfabs(s);
- r &= (t <=
+ r &= (std::abs(s) <=
(a[0] * Bf[2][1] + a[2] * Bf[0][1] +
b[0] * Bf[1][2] + b[2] * Bf[1][0]));
@@ -230,9 +217,8 @@ namespace PQP
// A1 x B2
s = T[0] * B[2][2] - T[2] * B[0][2];
- t = myfabs(s);
- r &= (t <=
+ r &= (std::abs(s) <=
(a[0] * Bf[2][2] + a[2] * Bf[0][2] +
b[0] * Bf[1][1] + b[1] * Bf[1][0]));
@@ -243,9 +229,8 @@ namespace PQP
// A2 x B0
s = T[1] * B[0][0] - T[0] * B[1][0];
- t = myfabs(s);
- r &= (t <=
+ r &= (std::abs(s) <=
(a[0] * Bf[1][0] + a[1] * Bf[0][0] +
b[1] * Bf[2][2] + b[2] * Bf[2][1]));
@@ -256,9 +241,8 @@ namespace PQP
// A2 x B1
s = T[1] * B[0][1] - T[0] * B[1][1];
- t = myfabs(s);
- r &= (t <=
+ r &= (std::abs(s) <=
(a[0] * Bf[1][1] + a[1] * Bf[0][1] +
b[0] * Bf[2][2] + b[2] * Bf[2][0]));
@@ -269,9 +253,8 @@ namespace PQP
// A2 x B2
s = T[1] * B[0][2] - T[0] * B[1][2];
- t = myfabs(s);
- r &= (t <=
+ r &= (std::abs(s) <=
(a[0] * Bf[1][2] + a[1] * Bf[0][2] +
b[0] * Bf[2][1] + b[1] * Bf[2][0]));
diff --git a/VirtualRobot/EndEffector/EndEffector.cpp b/VirtualRobot/EndEffector/EndEffector.cpp
index 5e2bfe366662924c64b4bff2688efd58cb0dffe3..8c2f3ce89e234a16fa9f1042f48fa83ef75a672e 100644
--- a/VirtualRobot/EndEffector/EndEffector.cpp
+++ b/VirtualRobot/EndEffector/EndEffector.cpp
@@ -115,12 +115,14 @@ namespace VirtualRobot
EndEffector::ContactInfoVector EndEffector::closeActors(SceneObjectSetPtr obstacles, float stepSize)
{
- std::vector<bool> actorCollisionStatus(actors.size(), false);
+ std::vector<char> actorCollisionStatus(actors.size(), false);
EndEffector::ContactInfoVector result;
bool finished = false;
int loop = 0;
+ const auto shared_this = shared_from_this();
+
while (!finished)
{
loop++;
@@ -133,10 +135,7 @@ namespace VirtualRobot
{
finished = false;
- if (actors[i]->moveActorCheckCollision(shared_from_this(), result, obstacles, stepSize))
- {
- actorCollisionStatus[i] = true;
- }
+ actorCollisionStatus[i] = actors[i]->moveActorCheckCollision(shared_this, result, obstacles, stepSize);
}
}
diff --git a/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp b/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp
index 0811b6c34c5c33b1caedef08c3841eb5cab28c28..5c2a130b8d65cea9ee39400c713a63eb8c96a676 100644
--- a/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp
+++ b/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp
@@ -99,31 +99,32 @@ namespace VirtualRobot
{
this->contactPoints.clear();
this->contactPointsM.clear();
- EndEffector::ContactInfoVector::const_iterator objPointsIter;
- for (objPointsIter = contactPoints.begin(); objPointsIter != contactPoints.end(); objPointsIter++)
+ this->contactPoints.reserve(contactPoints.size());
+ this->contactPointsM.reserve(contactPoints.size());
+
+ for (const auto& objPoint : contactPoints)
{
- MathTools::ContactPoint point;
+ this->contactPoints.emplace_back();
+ MathTools::ContactPoint& point = this->contactPoints.back();
- point.p = objPointsIter->contactPointObstacleLocal;
+ point.p = objPoint.contactPointObstacleLocal;
point.p -= centerOfModel;
- point.n = objPointsIter->contactPointFingerLocal - objPointsIter->contactPointObstacleLocal;
+ point.n = objPoint.contactPointFingerLocal - objPoint.contactPointObstacleLocal;
point.n.normalize();
// store force as projected component of approachDirection
- Eigen::Vector3f nGlob = objPointsIter->contactPointObstacleGlobal - objPointsIter->contactPointFingerGlobal;
+ const Eigen::Vector3f nGlob = objPoint.contactPointObstacleGlobal - objPoint.contactPointFingerGlobal;
if (nGlob.norm() > 1e-10)
{
- point.force = nGlob.dot(objPointsIter->approachDirectionGlobal) / nGlob.norm();
+ point.force = nGlob.dot(objPoint.approachDirectionGlobal) / nGlob.norm();
}
else
{
point.force = 0;
}
-
- this->contactPoints.push_back(point);
}
VirtualRobot::MathTools::convertMM2M(this->contactPoints, this->contactPointsM);
diff --git a/VirtualRobot/MathTools.cpp b/VirtualRobot/MathTools.cpp
index bfc0fb1a3ad10475a207975f6859fb5514ae034e..c2e77201448cd5233a35ca1faa403c8cef308c6c 100644
--- a/VirtualRobot/MathTools.cpp
+++ b/VirtualRobot/MathTools.cpp
@@ -849,15 +849,10 @@ namespace VirtualRobot
void VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::convertMM2M(const std::vector<ContactPoint> points, std::vector<ContactPoint>& storeResult)
{
float s = 1.0f / 1000.0f;
- storeResult.clear();
- std::vector<ContactPoint>::const_iterator iter1 = points.begin();
-
- while (iter1 != points.end())
+ storeResult = points;
+ for(auto& p : storeResult)
{
- ContactPoint tmp = *iter1;
- tmp.p *= s;
- storeResult.push_back(tmp);
- iter1++;
+ p.p *= s;
}
}