void Poly::draw_as_surface() const
{
vector<Triangle> triangles;
getTriangulation(triangles);
glBegin(GL_TRIANGLES);
for(size_t i=0;i<triangles.size();i++)
{
glNormal3dv((GLdouble*)&(triangles[i].Normal));
glVertex3dv((GLdouble*)&(triangles[i].A));
glVertex3dv((GLdouble*)&(triangles[i].B));
glVertex3dv((GLdouble*)&(triangles[i].C));
}
glEnd();
}
inline arma::Col<double>::fixed<6> ParallelKinematicMachine3PUPS::getEndEffectorPose(
const arma::Row<double>::fixed<3>& actuations,
const arma::Row<double>::fixed<3>& endEffectorRotation,
const arma::Row<double>& redundantJointActuations) const {
verify(arma::any(arma::vectorise(redundantJointActuations < 0)) || arma::any(arma::vectorise(redundantJointActuations > 1)), "All values for the actuation of redundantion joints must be between [0, 1].");
verify(redundantJointActuations.n_elem == redundantJointIndicies_.n_elem, "The number of redundant actuations must be equal to the number of redundant joints.");
const double& endEffectorRollAngle = endEffectorRotation(0);
const double& endEffectorPitchAngle = endEffectorRotation(1);
const double& endEffectorYawAngle = endEffectorRotation(2);
arma::Mat<double> baseJointPositions = redundantJointStartPositions_;
for (std::size_t n = 0; n < redundantJointIndicies_.n_elem; n++) {
const unsigned int& redundantJointIndex = redundantJointIndicies_(n);
baseJointPositions.col(redundantJointIndex) += redundantJointActuations(redundantJointIndex) * redundantJointStartToEndPositions_.col(redundantJointIndex);
}
baseJointPositions -= get3DRotation(endEffectorRollAngle, endEffectorPitchAngle, endEffectorYawAngle) * endEffectorJointPositions_;
return arma::join_cols(getTriangulation(baseJointPositions.col(0), actuations(0), baseJointPositions.col(1), actuations(1), baseJointPositions.col(2), actuations(2)), endEffectorRotation);
}
void Delaunay::generateDelaunay(const vector<Point2f> &pts,const vector<double>& attribute){
this->insert(pts);
getTriangulation(attribute);
std::sort(triangulation.begin(),triangulation.end(),compTri);//descend
}
