int bridgeAngle(SliceLayerPart* part, SliceLayer* prevLayer)
{
//To detect if we have a bridge, first calculate the intersection of the current layer with the previous layer.
// This gives us the islands that the layer rests on.
Polygons islands;
for(unsigned int n=0; n<prevLayer->parts.size(); n++)
{
if (!part->boundaryBox.hit(prevLayer->parts[n].boundaryBox)) continue;
islands.add(part->outline.intersection(prevLayer->parts[n].outline));
}
if (islands.size() > 5)
return -1;
//Next find the 2 largest islands that we rest on.
double area1 = 0;
double area2 = 0;
int idx1 = -1;
int idx2 = -1;
for(unsigned int n=0; n<islands.size(); n++)
{
//Skip internal holes
if (!ClipperLib::Orientation(islands[n]))
continue;
double area = fabs(ClipperLib::Area(islands[n]));
if (area > area1)
{
if (area1 > area2)
{
area2 = area1;
idx2 = idx1;
}
area1 = area;
idx1 = n;
}else if (area > area2)
{
area2 = area;
idx2 = n;
}
}
if (idx1 < 0 || idx2 < 0)
return -1;
Point center1 = centerOfMass(islands[idx1]);
Point center2 = centerOfMass(islands[idx2]);
double angle = atan2(center2.X - center1.X, center2.Y - center1.Y) / M_PI * 180;
if (angle < 0) angle += 360;
return angle;
}
int main(int argc, char *argv[]){
//check argc
if(argc!=4){
printf("Please enter the name of the input data file, the total time, and delta t in s\n");
}
double totalTime = atof(argv[2]);
double deltaT = atof(argv[3]);
body gravity[MAX_ENTRIES];
int i=0, count=0, k=0;
double cmx=0., cmy=0., cmz=0., j=0;
FILE *outp;
outp = fopen("location.dat", "w");
count=readData(argv[1], gravity);
centerOfMass(gravity, count, &cmx, &cmy, &cmz);
printf("The center of mass of all of the objects is: (%lf, %lf, %lf)\n", cmx, cmy, cmz);
gravForce(gravity, count);
for(i=0; i<count; i++){
printf("Body %3d Force = (%8.2lg, %8.2lg, %8.2lg)\n", i+1, gravity[i].f_vec[0],
gravity[i].f_vec[1], gravity[i].f_vec[2]);
}
for(j=0; j<totalTime; j+=deltaT){
gravForce(gravity, count);
evolve(gravity, count, deltaT);
for(k=0; k<count; k++){
fprintf(outp, "%lg %lg %lg\n", gravity[k].s_vec[0],
gravity[k].s_vec[1], gravity[k].s_vec[2]);
}
fprintf(outp, "\n\n");
}
fclose(outp);
return 0;
}
void SingleSupportModel::computeZMP()
{
if(m_coeff == 0)
{
ROS_ERROR("SingleSupportModel: computeZMP() was called with zero support coefficient");
return;
}
Math::SpatialVector spatialFootForce = -X_base[1].applyTranspose(f[1]);
double mass = link()->inertial->mass;
Eigen::Vector3d centerOfMass(
link()->inertial->origin.position.x,
link()->inertial->origin.position.y,
link()->inertial->origin.position.z
);
double F = -mass * 9.81;
double total_Fz = spatialFootForce[5] + F;
double total_Mx = spatialFootForce[0] + F * centerOfMass.y() - spatialFootForce[4] * nimbro_op_model::ANKLE_Z_HEIGHT;
double total_My = spatialFootForce[1] - F * centerOfMass.x() + spatialFootForce[3] * nimbro_op_model::ANKLE_Z_HEIGHT;
m_footForce = Eigen::Vector3d(0.0, 0.0, total_Fz);
m_zmp = Eigen::Vector3d(-total_My / total_Fz, total_Mx / total_Fz, 0);
// ZMP estimation based on the measured ankle roll torque
// TODO: Do this in a more general way?
Joint::Ptr ankle_roll = m_joints[0];
double total_Mx_torque = ankle_roll->feedback.torque + F * centerOfMass.y() - spatialFootForce[4] * nimbro_op_model::ANKLE_Z_HEIGHT;
m_zmp_torque = Eigen::Vector3d(-total_My / total_Fz, total_Mx_torque / total_Fz, 0);
}
Ogre::Vector3 OctreeSDF::getCenterOfMass(float& totalMass)
{
Ogre::Vector3 centerOfMass(0, 0, 0);
totalMass = 0;
m_RootNode->sumPositionsAndMass(m_RootArea, centerOfMass, totalMass);
if (totalMass > 0) centerOfMass /= totalMass;
return centerOfMass;
}
void Mesh::translateCenter(const Vec3& c)
{
for(int i = 0; i < numVtx(); ++i)
{
Vertex_handle v = find_vertex(i);
v->point() -= c;
}
compute_bounding_box();
centerOfMass();
}
static SE3::Vector3
com_0_proxy(const ModelHandler& model,
DataHandler & data,
const VectorXd_fx & q,
const bool updateKinematics = true)
{
return centerOfMass(*model,*data,q,
true,
updateKinematics);
}
void Mesh::transformVertices(const TrMatrix& tr)
{
for(Vertex_iterator it = vertices_begin(); it != vertices_end(); ++it)
{
Vec3& p = it->m_p;
p = tr.multVec(Vec4(p)).toVec();
}
// do some of the things done in finalize since all points were changed.
compute_normals_per_facet();
compute_normals_per_vertex();
compute_bounding_box();
compute_triangle_surfaces();
centerOfMass();
}
void Mesh::rescaleAndCenter(float destdialen)
{
Vec3 dia = m_max - m_min;
Vec3 center = (m_max + m_min) / 2.0;
float dialen = qMax(dia.x, dia.y);
float scale = destdialen/dialen;
for(int i = 0; i < numVtx(); ++i)
{
Vertex_handle v = find_vertex(i);
Vec3 &p = v->point();
p -= center;
p *= scale;
}
compute_bounding_box();
centerOfMass();
}
int main(int argc, char *argv[]){
//check argc
if(argc!=2){
printf("Please enter the name of the input data file\n");
}
body gravity[MAX_ENTRIES];
int i=0, count=0;
double cmx=0., cmy=0., cmz=0.;
count=readData(argv[1], gravity);
centerOfMass(gravity, count, &cmx, &cmy, &cmz);
printf("The center of mass of all of the objects is: (%lf, %lf, %lf)\n", cmx, cmy, cmz);
gravForce(gravity, count);
for(i=0; i<count; i++){
printf("Body %3d Force = (%8.2lg, %8.2lg, %8.2lg)\n", i+1, gravity[i].f_vec[0],
gravity[i].f_vec[1], gravity[i].f_vec[2]);
}
return 0;
}
void Mesh::finalize(bool needEdges)
{
buildVerticesInFaces();
if (!m_externalVtxNormals && !m_externalFaceNormals)
{
compute_normals_per_facet();
compute_normals_per_vertex();
}
else if (!m_externalFaceNormals)
{
average_normals_per_facet();
}
compute_bounding_box();
//mesh->estimateCurvature();
compute_triangle_surfaces();
centerOfMass();
//m_mesh->compute_volume();
if (needEdges)
buildEdges();
}
Vec3 Mesh::centerOfMass()
{
// compute for every face
for(int i = 0; i < numFaces(); ++i)
{
Face &f = m_face[i];
f.m_center.clear();
for(int i = 0; i < f.size(); ++i)
f.m_center += f.vertex(i)->point();
f.m_center /= f.size();
}
// now for all of the mess seperatly.
Vec3 centerOfMass(0,0,0);
for (Mesh::Vertex_iterator v = this->vertices_begin(); v != this->vertices_end(); v++)
{
centerOfMass = centerOfMass + (v->point()); // - CGAL::ORIGIN);
}
unsigned int size = this->size_of_vertices();
m_computedCenterOfMass = Vec3(centerOfMass.x/size, centerOfMass.y/size, centerOfMass.z/size);
return m_computedCenterOfMass;
}
const arma::mat& CylindricalBirefringentMaterial::inertiaTensor()
{
centerOfMass();
inertia.set_size(3,3);
inertia.fill(0.0);
for ( unsigned int z=0;z<Nz();z++ )
for ( unsigned int y=0;y<Ny();y++ )
for ( unsigned int x=0;x<Nx();x++ )
{
double xcrd = static_cast<int>(x)-com(0);
double ycrd = static_cast<int>(y)-com(1);
double zcrd = static_cast<int>(z)-com(2);
inertia(0,0) += (ycrd*ycrd + zcrd*zcrd)*get(x,y,z);
inertia(0,1) -= xcrd*ycrd*get(x,y,z);
inertia(0,2) -= xcrd*zcrd*get(x,y,z);
inertia(1,1) += (xcrd*xcrd+zcrd*zcrd)*get(x,y,z);
inertia(1,2) -= ycrd*zcrd*get(x,y,z);
inertia(2,2) += (xcrd*xcrd+ycrd*ycrd)*get(x,y,z);
}
inertia(1,0) = inertia(0,1);
inertia(2,1) = inertia(1,2);
inertia(2,0) = inertia(0,2);
return inertia;
}
void Molecule::moveToCOM()
{
Eigen::Vector3d com = centerOfMass();
this->translate(com);
}
void calculate(const homography_calc::matchedPoints& msg)
{
ROS_INFO("Received matched points");
std::vector<cv::Point2d> matched_kps_keyframe;
for (int i = 0; i < msg.keyframe_pts.size(); i++){
cv::Point2d pt(msg.keyframe_pts[i].x, msg.keyframe_pts[i].y);
matched_kps_keyframe.push_back(pt);
}
std::vector<cv::Point2d> matched_kps_moving;
for (int i = 0; i < msg.motion_pts.size(); i++){
cv::Point2d pt(msg.motion_pts[i].x, msg.motion_pts[i].y);
matched_kps_moving.push_back(pt);
}
cv::Mat moved_mat(matched_kps_moving); // TODO Look here (??)
cv::Mat keyframe_mat(matched_kps_keyframe);
if (matched_kps_moving.size() >= 4){
// Despite appearances, this isn't unnecessary. It's possible to remove points
// via RANSAC and then not have enough to compute the homography (also >=4 points required)
// std::cout << "here " << std::endl;
double H_array[9];
findHomographyHomest( matched_kps_moving, matched_kps_keyframe, H_array );
cv::Mat H = cv::Mat(3,3,CV_64F, &H_array);
// printf("H is %f %f %f ", H_array[0], H_array[1], H_array[2]);
// std::cout << H << std::endl;
/*****************************
Calculate reprojection error and define new keyframe if necessary.
*****************************/
std::vector<cv::Point2d> keyframe_reproj;
cv::perspectiveTransform( matched_kps_moving, keyframe_reproj, H);
// keyframe_reproj = H * matched_keypoints_moved;
double err = 0;
for (int i = 0; i < matched_kps_moving.size(); i++){
err += cv::norm(cv::Mat(matched_kps_keyframe[i] ), cv::Mat(keyframe_reproj[i] ) );
}
err = err / matched_kps_moving.size();
std::cout << "Reprojection error is " << err << std::endl;
double kfResetThresh;
if (ros::param::has("keyframeResetThresh") ){
ros::param::get("keyframeResetThresh", kfResetThresh);
}else{
kfResetThresh = 30;
}
if (err > kfResetThresh){
ROS_INFO("Resetting the keyframe NOW");
std_msgs::Bool resetMsg;
resetMsg.data = true;
resetPub.publish(resetMsg);
}
//Publish the homography
std_msgs::Float64MultiArray homography;
for(int i = 0 ; i < 3; i++){
for (int j = 0; j < 3; j++){
homography.data.push_back(H.at<float>(i, j) );
}
}
homogPub.publish(homography);
Eigen::MatrixXd currentCenter(3,1);
Eigen::MatrixXd targetCenter(3,1);
currentCenter = centerOfMass(matched_kps_moving);
targetCenter = centerOfMass(matched_kps_keyframe);
homography_calc::controlLawInputs controlMsg;
controlMsg.homography = homography;
controlMsg.currentCenterOfMass.x = currentCenter(0,0);
controlMsg.currentCenterOfMass.y = currentCenter(0,1);
controlMsg.currentCenterOfMass.z = currentCenter(0,2);
controlMsg.targetCenterOfMass.x = targetCenter(0,0);
controlMsg.targetCenterOfMass.y = targetCenter(0,1);
controlMsg.targetCenterOfMass.z = targetCenter(0,2);
homogControlPub.publish(controlMsg);
}
}
void RobotModel::visualizeData(visualization_msgs::MarkerArray* markers)
{
ros::Time now = ros::Time::now();
tf::Vector3 com = centerOfMass();
visualization_msgs::Marker com_marker;
com_marker.header.stamp = now;
com_marker.header.frame_id = "/trunk_link";
com_marker.ns = "robotcontrol";
com_marker.id = 0;
com_marker.type = visualization_msgs::Marker::SPHERE;
com_marker.action = visualization_msgs::Marker::ADD;
com_marker.pose.position.x = com.x();
com_marker.pose.position.y = com.y();
com_marker.pose.position.z = com.z();
com_marker.pose.orientation.w = 1;
com_marker.pose.orientation.x = 0;
com_marker.pose.orientation.y = 0;
com_marker.pose.orientation.z = 0;
com_marker.scale.x = 0.05;
com_marker.scale.y = 0.05;
com_marker.scale.z = 0.05;
com_marker.color.a = 1.0;
com_marker.color.r = 1.0;
com_marker.color.g = 0.0;
com_marker.color.b = 0.0;
markers->markers.push_back(com_marker);
visualization_msgs::Marker magnetic_marker;
magnetic_marker.header.stamp = now;
magnetic_marker.header.frame_id = "/trunk_link";
magnetic_marker.ns = "robotcontrol";
magnetic_marker.id = 1;
magnetic_marker.type = visualization_msgs::Marker::ARROW;
magnetic_marker.action = visualization_msgs::Marker::ADD;
magnetic_marker.points.resize(2);
tf::pointEigenToMsg(Eigen::Vector3d::Zero(), magnetic_marker.points[0]);
tf::pointEigenToMsg(m_magneticFieldVector, magnetic_marker.points[1]);
magnetic_marker.scale.x = 0.01;
magnetic_marker.scale.y = 0.02;
magnetic_marker.scale.z = 0.0;
magnetic_marker.color.a = 1.0;
magnetic_marker.color.r = 0.0;
magnetic_marker.color.g = 1.0;
magnetic_marker.color.b = 0.0;
markers->markers.push_back(magnetic_marker);
// Plot support coefficients
plot_msgs::Plot plot;
plot.header.stamp = now;
plot.points.resize(m_models.size());
for(size_t i = 0; i < m_models.size(); ++i)
{
plot.points[i].name = "Support/" + m_models[i]->link()->name;
plot.points[i].value = m_models[i]->coefficient();
}
m_pub_plot.publish(plot);
m_pub_plot_lastTime = now;
}
int
SOP_PrimGroupCentroid::bindToCentroids(fpreal t, int mode, int method)
{
int behavior;
exint int_value;
const GA_PrimitiveGroup *group;
GA_PrimitiveGroup *all_prims, *temp_group;
GA_Range pr_range;
GA_ROAttributeRef attr_gah, primattr_gah;
GA_ROHandleI class_h;
GA_ROHandleS str_h;
const GU_Detail *input_geo;
UT_Matrix4 mat;
UT_String attr_name, pattern, str_value;
UT_Vector3 pos;
// Get the second input geometry as read only.
GU_DetailHandleAutoReadLock gdl(inputGeoHandle(1));
input_geo = gdl.getGdp();
// Get the unmatched geometry behavior.
behavior = BEHAVIOR(t);
// Create a new attribute reference map.
GA_AttributeRefMap hmap(*gdp, input_geo);
// Get the attribute selection string.
BIND(pattern, t);
// If we have a pattern, try to build the ref map.
if (pattern.length() > 0)
buildRefMap(hmap, pattern, gdp, input_geo, mode, GA_ATTRIB_POINT);
// The list of GA_Primitives in the input geometry.
const GA_PrimitiveList &prim_list = gdp->getPrimitiveList();
// Create a temporary primitive group so we can keep track of all the
// primitives we have modified.
all_prims = createAdhocPrimGroup(*gdp, "allprims");
// Determine which attribute we need from the points, based on the mode.
switch (mode)
{
case 0:
attr_name = "group";
break;
case 1:
attr_name = "name";
break;
case 2:
attr_name = "class";
break;
default:
addError(SOP_MESSAGE, "Invalid mode setting");
return 1;
}
// Find the attribute.
attr_gah = input_geo->findPointAttribute(attr_name);
// If there is no attribute, add an error message and quit.
if (attr_gah.isInvalid())
{
addError(SOP_ATTRIBUTE_INVALID, attr_name);
return 1;
}
// If not using groups, we need to check if the matching primitive
// attribute exists on the geometry.
if (mode != 0)
{
// Try to find the attribute.
primattr_gah = gdp->findPrimitiveAttribute(attr_name);
// If there is no attribute, add an error message and quit.
if (primattr_gah.isInvalid())
{
addError(SOP_ATTRIBUTE_INVALID, attr_name);
return 1;
}
}
// 'class' uses the int handle.
if (mode == 2)
class_h.bind(attr_gah.getAttribute());
// Groups and 'name' use the string handle.
else
str_h.bind(attr_gah.getAttribute());
for (GA_Iterator it(input_geo->getPointRange()); !it.atEnd(); ++it)
{
if (mode == 0)
{
// Get the unique string value.
str_value = str_h.get(*it);
// Find the group on the geometry to bind.
group = gdp->findPrimitiveGroup(str_value);
// Ignore non-existent groups.
if (!group)
continue;
// Skip emptry groups.
if (group->isEmpty())
continue;
// The primtives in the group.
pr_range = gdp->getPrimitiveRange(group);
}
else
{
if (mode == 1)
{
// Get the unique string value.
str_value = str_h.get(*it);
// Get the prims with that string value.
pr_range = gdp->getRangeByValue(primattr_gah, str_value);
}
else
{
// Get the unique integer value.
int_value = class_h.get(*it);
// Get the prims with that integery value.
pr_range = gdp->getRangeByValue(primattr_gah, int_value);
}
// Create an adhoc group.
temp_group = createAdhocPrimGroup(*gdp);
temp_group->addRange(pr_range);
}
// Add the primitives in the range to the groups.
all_prims->addRange(pr_range);
// Bounding Box
if (method == 1)
{
// Calculate the bouding box center for this range.
boundingBox(gdp, pr_range, prim_list, pos);
}
// Center of Mass
else if (method == 2)
{
// Calculate the center of mass for this attribute value.
centerOfMass(pr_range, prim_list, pos);
}
// Barycenter
else
{
// Calculate the barycenter for this attribute value.
baryCenter(gdp, pr_range, prim_list, pos);
}
// Build the transform from the point information.
buildTransform(mat, input_geo, pos, *it);
// Transform the geometry from the centroid.
if (mode == 0)
gdp->transform(mat, group);
else
gdp->transform(mat, temp_group);
// Copy any necessary attributes from the incoming points to the
// geometry.
if (hmap.entries())
{
for (GA_Iterator pr_it(pr_range); !pr_it.atEnd(); ++pr_it)
{
hmap.copyValue(GA_ATTRIB_PRIMITIVE,
*pr_it,
GA_ATTRIB_POINT,
*it);
}
}
}
// We want to destroy prims that didn't have a matching name/group.
if (behavior)
{
// Flip the membership of all the prims that we did see.
all_prims->toggleEntries();
// Destroy the ones that we didn't.
gdp->deletePrimitives(*all_prims, true);
}
return 0;
}
int
SOP_PrimGroupCentroid::buildCentroids(fpreal t, int mode, int method)
{
bool store;
exint int_value;
const GA_AIFStringTuple *ident_t;
GA_Attribute *ident_attrib;
GA_Offset ptOff;
GA_RWAttributeRef ident_gah;
GA_RWHandleI class_h;
const GU_Detail *input_geo;
UT_BoundingBox bbox;
UT_String attr_name, pattern, str_value;
UT_Vector3 pos;
UT_Array<GA_Range> range_array;
UT_Array<GA_Range>::const_iterator array_it;
UT_StringArray string_values;
UT_IntArray int_values;
// Get the input geometry as read only.
GU_DetailHandleAutoReadLock gdl(inputGeoHandle(0));
input_geo = gdl.getGdp();
// Check to see if we should store the source group/attribute name as an
// attribute the generated points.
store = STORE(t);
// If we want to we need to create the attributes.
if (store)
{
// A 'class' operation, so create a new integer attribute.
if (mode == 2)
{
// Add the int tuple.
ident_gah = gdp->addIntTuple(GA_ATTRIB_POINT, "class", 1);
// Bind the handle.
class_h.bind(ident_gah.getAttribute());
}
// Using the 'name' attribute or groups, so create a new string
// attribute.
else
{
attr_name = (mode == 0) ? "group" : "name";
// Create a new string attribute.
ident_gah = gdp->addStringTuple(GA_ATTRIB_POINT, attr_name, 1);
ident_attrib = ident_gah.getAttribute();
// Get the string tuple so we can set values.
ident_t = ident_gah.getAIFStringTuple();
}
}
// Create a new attribute reference map.
GA_AttributeRefMap hmap(*gdp, input_geo);
// Get the attribute selection string.
ATTRIBUTES(pattern, t);
// If we have a pattern, try to build the ref map.
if (pattern.length() > 0)
buildRefMap(hmap, pattern, gdp, input_geo, mode, GA_ATTRIB_PRIMITIVE);
// The list of GA_Primitives in the input geometry.
const GA_PrimitiveList &prim_list = input_geo->getPrimitiveList();
// Creating by groups.
if (mode == 0)
{
// Get the group pattern.
GROUP(pattern, t);
// If the group string is empty, get out of here.
if (pattern.length() == 0)
return 1;
buildGroupData(pattern, input_geo, range_array, string_values);
}
// 'name' or 'class'.
else
{
// Build the data. If something failed, return that we had an issue.
if (buildAttribData(mode, input_geo, range_array, string_values, int_values))
return 1;
}
// Iterate over each of the primitive ranges we found.
for (array_it=range_array.begin(); !array_it.atEnd(); ++array_it)
{
// Create a new point.
ptOff = gdp->appendPointOffset();
// Bounding Box
if (method == 1)
{
// Calculate the bouding box center for this range.
boundingBox(input_geo, *array_it, prim_list, pos);
// Set the point's position to the center of the box.
gdp->setPos3(ptOff, pos);
}
// Center of Mass
else if (method == 2)
{
// Calculate the center of mass for this range.
centerOfMass(*array_it, prim_list, pos);
// Set the point's position to the center of mass.
gdp->setPos3(ptOff, pos);
}
// Barycenter
else
{
// Calculate the barycenter for this range.
baryCenter(input_geo, *array_it, prim_list, pos);
// Set the point's position to the barycenter.
gdp->setPos3(ptOff, pos);
}
// Store the source value if required.
if (store)
{
// 'class', so get the integer value at this iterator index.
if (mode == 2)
{
int_value = int_values(array_it.index());
class_h.set(ptOff, int_value);
}
// 'name' or by group, so get the string value at this iterator
// index.
else
{
str_value = string_values(array_it.index());
ident_t->setString(ident_attrib, ptOff, str_value, 0);
}
}
// If there are no entries in the map then we don't need to copy
// anything.
if (hmap.entries() > 0)
{
GA_WeightedSum sum;
// Start a weighted sum for the range.
hmap.startSum(sum, GA_ATTRIB_POINT, ptOff);
// Add the values for each primitive to the sum.
for (GA_Iterator it(*array_it); !it.atEnd(); ++it)
{
hmap.addSumValue(sum,
GA_ATTRIB_POINT,
ptOff,
GA_ATTRIB_PRIMITIVE,
*it,
1);
}
// Finish the sum, normalizing the values.
hmap.finishSum(sum,
GA_ATTRIB_POINT,
ptOff,
1.0/(*array_it).getEntries());
}
}
return 0;
}
// Test the function of center of mass
bool testCenterOfMass(){
bool result = true;
// Make lists of masses and xy coordinates
int N = 7;
value_type xsorted[N] = {0.2, 0.25, 0.4, 0.48, 0.7, 0.72, 0.8};
value_type ysorted[N] = {0.3, 0.2, 0.2, 0.1, 0.7, 0.4, 0.4};
value_type masssorted[N] = {0.4, 0.3, 0.3, 0.89, 0.41, 0.1, 0.66};
// Initiate the children
Node children[4];
// Initialize the children nodes
Node child_0 = Node { 1, // level
0, // morton index
-1, // child_id
0, // part_start
2, // part_end
1, // node mass
1, // x center of mass
1 // y center of mass
};
Node child_1 = Node { 1, // level
0, // morton index
-1, // child_id
3, // part_start
4, // part_end
1, // node mass
1, // x center of mass
1 // y center of mass
};
Node child_2 = Node { 1, // level
0, // morton index
-1, // child_id
5, // part_start
5, // part_end
1, // node mass
1, // x center of mass
1 // y center of mass
};
Node child_3 = Node { 1, // level
0, // morton index
-1, // child_id
6, // part_start
6, // part_end
1, // node mass
1, // x center of mass
1 // y center of mass
};
// Put the children nodes into the array
children[0] = child_0;
children[1] = child_1;
children[2] = child_2;
children[3] = child_3;
centerOfMass(children, xsorted, ysorted, masssorted);
// Test the values against known true values
value_type xcom_true[4] = {0.275, 0.5493846, 0.72, 0.8};
value_type ycom_true[4] = {0.24, 0.2892307, 0.4, 0.4};
value_type mass_true[4] = {1, 1.3, 0.1, 0.66};
for (int i = 0; i < 4; ++i) {
if (children[i].mass - mass_true[i] > 0.000001) {
std::cout << "Mass of node " << i << " computed incorrectly." << std::endl;
result = false;
}
if (children[i].xcom - xcom_true[i] > 0.000001) {
std::cout << "xcom of node " << i << " computed incorrectly." << std::endl;
result = false;
}
if (children[i].ycom - ycom_true[i] > 0.000001) {
std::cout << "ycom of node " << i << " computed incorrectly." << std::endl;
result = false;
}
}
if (result) {
std::cout << "Test succeeded." << std::endl;
}
return result;
}
void sefield::TetMesh::axisOrderElements(uint opt_method, std::string const & opt_file_name)
{
// Now this method provides a choice between Stefan and Robert's method
// and the new method by Iain. The original method is fast and suffices for
// simple geometries, Iain's method is superior and important for complex
// geometries, but slow.
if (opt_file_name != "")
{
std::fstream opt_file;
opt_file.open(opt_file_name.c_str(),
std::fstream::in | std::fstream::binary);
opt_file.seekg(0);
uint nelems = 0;
opt_file.read((char*)&nelems, sizeof(uint));
if (pElements.size() != nelems) {
std::ostringstream os;
os << "optimal data mismatch with simulator parameters: sefield::Tetmesh::nelems, ";
os << nelems << ":" << pElements.size();
throw steps::ArgErr(os.str());
}
opt_file.read((char*)pVertexPerm, sizeof(uint) * nelems);
VertexElementPVec elements_temp = pElements;
for (uint vidx = 0; vidx < nelems; ++vidx)
{
VertexElementP vep = elements_temp[vidx];
// sanity check
assert(vep->getIDX() == vidx);
uint new_idx = pVertexPerm[vidx];
pElements[new_idx] = vep;
}
reindexElements();
reordered();
opt_file.close();
return;
}
if (opt_method == 2)
{
// / / / / / / / / / / / / / / NEW / / / / / / / / / / / / / / / / / / //
// LOOPING OVER ALL VERTICES, APPLYING THE WALK METHOD AND FINDING WHICH
// STARTING VERTEX GIVES THE LOWEST MATRIX WIDTH.
uint pNVerts = pElements.size();
// the best vertex(narrowest width)
uint bestone = 0;
uint bestwidth = pNVerts;
std::vector<VertexElement*> orig_indices = pElements;
stringstream ss;
ss << "\nFinding optimal vertex indexing. This can take some time...";
cout << ss.str() << endl;
for (uint vidx = 0; vidx < pNVerts; ++vidx)
{
set<VertexElement*> verteleset = set<VertexElement*>();
vector<VertexElement*> vertelevec = vector<VertexElement*>();
queue<VertexElement*> vertelqueue = queue<VertexElement*>();
verteleset.insert(orig_indices[vidx]);
vertelevec.push_back(orig_indices[vidx]);
uint ve0ncons = orig_indices[vidx]->getNCon();
VertexElement ** ve0neighbs = orig_indices[vidx]->getNeighbours();
fill_ve_vec(verteleset, vertelevec, vertelqueue, ve0ncons, ve0neighbs);
pElements.clear();
vector<VertexElement*>::iterator vertele_end = vertelevec.end();
uint ielt = 0;
for (vector<VertexElement*>::iterator vertele = vertelevec.begin(); vertele != vertele_end; ++vertele)
{
pElements.push_back(*vertele);
pVertexPerm[(*vertele)->getIDX()]=ielt;
ielt++;
}
// Note: this will reorder the vertex indices as we go- not a problem in this loop
// but they must be reset for the final index setting to work
reindexElements();
uint maxdi = 0;
for (int iv = 0; iv < pNVerts; ++iv)
{
VertexElement* ve = getVertex(iv);
int ind = ve->getIDX();
for (int i = 0; i < ve->getNCon(); ++i)
{
int inbr = ve->nbrIdx(i);
int di = ind - inbr;
if (di < 0)
{
di = -di;
}
if (di > maxdi)
{
maxdi = di;
}
}
}
if (maxdi < bestwidth)
{
bestone = vidx;
bestwidth = maxdi;
//cout << "\nOriginal vertex "<< vidx << " gives banded matrix half-width " << maxdi;
}
}
// reset the vertex indices to their original value: important
for (uint v = 0; v < pNVerts; ++v)
{
orig_indices[v]->setIDX(v);
}
set<VertexElement*> verteleset = set<VertexElement*>();
vector<VertexElement*> vertelevec = vector<VertexElement*>();
queue<VertexElement*> vertelqueue = queue<VertexElement*>();
verteleset.insert(orig_indices[bestone]);
vertelevec.push_back(orig_indices[bestone]);
uint ve0ncons = orig_indices[bestone]->getNCon();
VertexElement ** ve0neighbs = orig_indices[bestone]->getNeighbours();
fill_ve_vec(verteleset, vertelevec, vertelqueue, ve0ncons, ve0neighbs);
pElements.clear();
vector<VertexElement*>::iterator vertele_end = vertelevec.end();
uint ielt = 0;
for (vector<VertexElement*>::iterator vertele = vertelevec.begin(); vertele != vertele_end; ++vertele)
{
pElements.push_back(*vertele);
pVertexPerm[(*vertele)->getIDX()]=ielt;
ielt++;
}
}
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / //
else if (opt_method == 1)
{
/*
THIS ORIGINAL CODE REPLACED WITH NEW 'WALKING' METHOD. A DRAMATIC
REDUCTION IN BAND HALF-WIDTHS FOR COMPLEX 3D MESHES.
This is not the end of the story: TODO investigate whether reordering
a vertices neighbour's for the queue by distance will improve again.
*/
std::vector<double> com = centerOfMass();
double ** m = new double*[3];
for (uint i = 0; i < 3; i++)
{
m[i] = new double[3];
m[i][0] = 0.0;
m[i][1] = 0.0;
m[i][2] = 0.0;
}
for (uint ivert = 0; ivert < countVertices(); ivert++)
{
VertexElement* ve = getVertex(ivert);
double x = ve->getX() - com[0];
double y = ve->getY() - com[1];
double z = ve->getZ() - com[2];
m[0][0] += x * x;
m[0][1] += x * y;
m[0][2] += x * z;
m[1][0] += y * x;
m[1][1] += y * y;
m[1][2] += y * z;
m[2][0] += z * x;
m[2][1] += z * y;
m[2][2] += z * z;
}
uint iret;
double gamma;
double evec[3];
mainEvec(3, m, &iret, &gamma, evec);
if (iret != 1)
{
cout << "\nWarning - eigenvalue faliure " << endl;
}
for (uint i = 0; i < 3; i++)
{
delete[] m[i];
}
delete[] m;
double vx = evec[0];
double vy = evec[1];
double vz = evec[2];
vx += 1.e-8;
vy += 1.e-9;
vz += 1.e-10;
stringstream ss;
ss << "aligning to axis " << vx << " " << vy << " " << vz << endl;
//cout << ss.str();
map<double, VertexElement*> hm;
//vector<double> da;
for (uint ielt = 0; ielt < countVertices(); ielt++)
{
VertexElement * ve = getVertex(ielt);
double d = vx * ve->getX() + vy * ve->getY() + vz * ve->getZ();
hm[d] = ve;
//da.push_back(d);
}
//sort(da.begin(), da.end());
pElements.clear();
uint ielt = 0;
map<double, VertexElement*>::const_iterator iter = hm.begin();
while (iter != hm.end())
{
double d = iter->first;
pElements.push_back(hm[d]);
++iter;
// pVertexPerm[i] contains the new index in the elements array
// for the vertex that was originally at index i
pVertexPerm[hm[d]->getIDX()] = ielt;
ielt++;
}
}
else
{
std::ostringstream os;
os << "Unknown optimization method.\n";
throw steps::ArgErr(os.str());
}
reindexElements();
reordered();
}
GeneralPolygon::operator list<TPPLPoly>()
{
auto isPolygonOutside = [&](const Contour &referencePolygon, const Contour &poly)
{
for(unsigned int p=0; p < referencePolygon.size() ; ++p)
if( !_evenOddRuleAlgorithm( referencePolygon[p], poly) )
return true;
return false;
};
list<TPPLPoly> polys;
for(unsigned int c=0; c < _contours.size() ; ++c)
{
const Contour &contour = _contours[c];
TPPLPoly poly;
poly.Init(contour.size());
for(unsigned int v=0; v < contour.size() ; ++v)
{
TPPLPoint point;
point.x = contour[v].x;
point.y = contour[v].y;
poly[v] = point;
}
// BE CAREFULL!!! not really correct because the polygon could be concave then
// the center of mass could not be inside the polygon
sf::Vector2f centerOfMass(0.0f, 0.0f);
for(unsigned int i=0; i < contour.size() ; ++i)
centerOfMass += contour[i];
centerOfMass *= (1.0f/contour.size());
std::vector<Contour> outsideContours;
for(unsigned int i=0; i < _contours.size() ; ++i)
{
if( i == c )
{
outsideContours.push_back(_contours[i]);
continue;
}
if( isPolygonOutside(_contours[i], contour) )
outsideContours.push_back(_contours[i]);
}
// first test if is a hole or not
if( !_evenOddRuleAlgorithm( centerOfMass, outsideContours ) )
poly.SetHole(true);
else
poly.SetHole(false);
// if it is a hole then it must be in CW order to the algorithm to recognize
// else must be in CCW
if( poly.IsHole() )
poly.SetOrientation(TPPL_CW);// Hole orientation (needed in the algorithm)
else
poly.SetOrientation(TPPL_CCW);// Not a hole orientation (needed in the algorithm)
polys.push_back(poly);
}
return polys;
}
/*
* A function that splits the parent node and creates 4 new children nodes in the array of nodes named "tree".
*/
void split(Node* parent, Node* tree, int depth, unsigned int* index, value_type* xsorted, value_type* ysorted, value_type* mass_sorted, int k, int* newNodeIndex){
// Capture and update the newNodeIndex atomically to avoid race condition
/* In case of omp tasking make this atomic to avoid race conditions
* #pragma omp atomic capture
{} */
parent->child_id = *newNodeIndex;
*newNodeIndex += 4;
// Compute the level of the children
unsigned int children_level = parent->level +1;
// Compute the indexvalue of this level so we can easily compute the morton-id's of the children
int indexValue_level = pow(2,2*(depth - children_level));
// Allocate pointers for the expansions arrays
value_type * rxps0;
value_type * ixps0;
value_type * rxps1;
value_type * ixps1;
value_type * rxps2;
value_type * ixps2;
value_type * rxps3;
value_type * ixps3;
// Initialize the children nodes
Node child_0 = Node {children_level, // level
parent->morton_id, // morton index
-1, // child_id
-1, // part_start
-1, // part_end
1, // node mass
1, // x center of mass
1, // y center of mass
NAN, // radius of node
rxps0, // real part of multipole expansion
ixps0 // imaginary part of multipole expansion
};
Node child_1 = Node {children_level, // level
parent->morton_id + indexValue_level, // morton index
-1, // child_id
-1, // part_start
-1, // part_end
1, // node mass
1, // x center of mass
1, // y center of mass
NAN, // radius of node
rxps1, // real part of multipole expansion
ixps1 // imaginary part of multipole expansion
};
Node child_2 = Node {children_level, // level
parent->morton_id + 2*indexValue_level, // morton index
-1, // child_id
-1, // part_start
-1, // part_end
1, // node mass
1, // x center of mass
1, // y center of mass
NAN, // radius of node
rxps2, // real part of multipole expansion
ixps2 // imaginary part of multipole expansion
};
Node child_3 = Node {children_level, // level
parent->morton_id + 3*indexValue_level, // morton index
-1, // child_id
-1, // part_start
-1, // part_end
1, // node mass
1, // x center of mass
1, // y center of mass
NAN, // radius of node
rxps3, // real part of multipole expansion
ixps3 // imaginary part of multipole expansion
};
if(parent->level == 0){
// Print the maximum index
unsigned int max = index[0];
int min = 0;
for (int l = 0; l < parent->part_end - parent->part_start; ++l) {
if(index[l]>max){
max = index[l];
}
if(index[l] < min){
min = index[l];
}
}
// std::cout << "Biggest Morton Index = " << max << std::endl;
// std::cout << "Smallest Morton Index = " << min << std::endl;
// std::cout << "Morton limit tree = " << child_3.morton_id - 1 + indexValue_level << std::endl;
// std::cout << "IndexValue at level = " << children_level << " is " << indexValue_level << std::endl;
}
// Set a pointer to the first child node
Node* children = tree + parent->child_id;
// Put the children nodes into the array at indices [ children[0], ... , children[3] ].
children[0] = child_0;
children[1] = child_1;
children[2] = child_2;
children[3] = child_3;
// Assign the particles to the children
assignParticles(parent, children, depth, index);
// Assign the total and center of mass to the children, as well as their radius r
centerOfMass(children, xsorted, ysorted, mass_sorted);
radius(children, xsorted, ysorted);
// Compute the multipole expansions for the children nodes, only if the level is 2 or deeper and if the node is not empty
if(children_level >= 2){
int nParticlesChild = 0;
for (int c = 0; c < 4; ++c) {
// Check if this child node is empty or contains only 1 particle.
if(children[c].part_start == children[c].part_end){
// Do nothing
} else {
// Compute the number of particles in this child node
nParticlesChild = children[c].part_end - children[c].part_start + 1;
// Align expansion arrays
posix_memalign((void **) &children[c].rxps, 32, sizeof(value_type) * exp_order);
posix_memalign((void **) &children[c].ixps, 32, sizeof(value_type) * exp_order);
// Compute and set the expansion in this child node
p2e(xsorted + children[c].part_start, // x values of child's particles
ysorted + children[c].part_start, // y values of child's particles
mass_sorted + children[c].part_start, // mass values of child's particles
nParticlesChild, // number of particles in child
exp_order, // order of expansion
children[c].xcom, // x value center of mass
children[c].ycom, // y value center of mass
children[c].rxps, // real parts of expansion
children[c].ixps); // imaginary parts of expansion
/* Print info about the computed expansion
std::cout << "Child node " << c << " has expansion:" << std::endl;
for (int i = 0; i < exp_order; ++i) {
std::cout << "rxps [" << i << "] = " << children[c].rxps[i] << std::endl;
std::cout << "ixps [" << i << "] = " << children[c].ixps[i] << "\n" << std::endl;
} */
}
}
}
// Check number of particles in the children nodes
for (int i = 0; i < 4; ++i) {
if (children[i].part_end - children[i].part_start + 1 > k && children[i].level < depth) {
// There are more than k particles in the node and we haven't reached the maximum depth so split it in four
split(children + i, tree, depth, index, xsorted, ysorted, mass_sorted, k, newNodeIndex);
}
else {
// There are less than or equal to k particles in the node
// or we reached the maximum depth => the node is a leaf
}
}
}
