/**
* Ensures that we have a vertex to process.
*
* @return the vertex to process
*/
std::vector<float> &
Mesh::ensure_vertex()
{
if (vertices_.empty())
next_vertex();
return vertices_.back();
}
/**
* Creates a grid mesh.
*
* @param n_x the number of grid cells on the X axis
* @param n_y the number of grid cells on the Y axis
* @param width the width X of the grid (normalized)
* @param height the height Y of the grid (normalized)
* @param spacing the spacing between cells (normalized)
* @param conf_func a function to call to configure the grid (or NULL)
*/
void
Mesh::make_grid(int n_x, int n_y, double width, double height,
double spacing, grid_configuration_func conf_func)
{
double side_width = (width - (n_x - 1) * spacing) / n_x;
double side_height = (height - (n_y - 1) * spacing) / n_y;
for (int i = 0; i < n_x; i++) {
for (int j = 0; j < n_y; j++) {
LibMatrix::vec3 a(-width / 2 + i * (side_width + spacing),
height / 2 - j * (side_height + spacing), 0);
LibMatrix::vec3 b(a.x(), a.y() - side_height, 0);
LibMatrix::vec3 c(a.x() + side_width, a.y(), 0);
LibMatrix::vec3 d(a.x() + side_width, a.y() - side_height, 0);
if (!conf_func) {
std::vector<float> ul(vertex_size_);
std::vector<float> ur(vertex_size_);
std::vector<float> ll(vertex_size_);
std::vector<float> lr(vertex_size_);
set_attrib(0, a, &ul);
set_attrib(0, c, &ur);
set_attrib(0, b, &ll);
set_attrib(0, d, &lr);
next_vertex(); vertices_.back() = ul;
next_vertex(); vertices_.back() = ll;
next_vertex(); vertices_.back() = ur;
next_vertex(); vertices_.back() = ll;
next_vertex(); vertices_.back() = lr;
next_vertex(); vertices_.back() = ur;
}
else {
conf_func(*this, i, j, n_x, n_y, a, b, c, d);
}
}
}
}
void World::initRestingThread(int opt)
{
int numInit = 5;
double first_length = 3.0; //FIRST_REST_LENGTH;
double second_length = SECOND_REST_LENGTH;
double middle_length = DEFAULT_REST_LENGTH;
vector<Vector3d> vertices;
vector<double> angles;
vector<double> lengths;
vector<Vector3d> directions;
if (opt == 0 || opt == 1) {
for (int i=0; i < 4*numInit + 5; i++)
angles.push_back(0.0);
lengths.push_back(first_length);
lengths.push_back(second_length);
for (int i=0; i < 4*numInit; i++)
lengths.push_back(middle_length);
lengths.push_back(second_length);
lengths.push_back(first_length);
lengths.push_back(first_length);
directions.push_back(Vector3d::UnitX());
directions.push_back(Vector3d::UnitX());
Vector3d direction = Vector3d(1.0, 1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit; i++)
directions.push_back(direction);
direction = Vector3d(1.0, -1.0, 0.4);
direction.normalize();
for (int i=0; i < numInit; i++)
directions.push_back(direction);
direction = Vector3d(-1.0, -1.0, -0.4);
direction.normalize();
for (int i=0; i < numInit; i++)
directions.push_back(direction);
direction = Vector3d(0.0, -1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit; i++)
directions.push_back(direction);
directions.push_back(-Vector3d::UnitY());
directions.push_back(-Vector3d::UnitY());
vertices.push_back(Vector3d::Zero());
for (int i=1; i < 4*numInit + 5; i++) {
Vector3d next_vertex;
next_vertex(0) = vertices[i-1](0) + directions[i-1](0) * lengths[i-1];
next_vertex(1) = vertices[i-1](1) + directions[i-1](1) * lengths[i-1];
next_vertex(2) = vertices[i-1](2) + directions[i-1](2) * lengths[i-1];
vertices.push_back(next_vertex);
}
Vector3d last_pos = Vector3d(-10.0, -30.0, 0.0);
for (int i=0; i<vertices.size(); i++)
vertices[i] += -vertices.back() + last_pos;
Matrix3d start_rotation0 = Matrix3d::Identity();
Matrix3d end_rotation0 = (Matrix3d) AngleAxisd(-M_PI/2.0, Vector3d::UnitZ());
ThreadConstrained* thread0 = new ThreadConstrained(vertices, angles, lengths, start_rotation0, end_rotation0, this);
threads.push_back(thread0);
}
if (opt == 0 || opt == 2) {
int numInit1 = 2;
angles.clear();
for (int i=0; i < 16*numInit1 + 5; i++)
angles.push_back(0.0);
lengths.clear();
lengths.push_back(first_length);
lengths.push_back(second_length);
for (int i=0; i < 16*numInit1; i++)
lengths.push_back(middle_length);
lengths.push_back(second_length);
lengths.push_back(first_length);
lengths.push_back(first_length);
directions.clear();
directions.push_back(-Vector3d::UnitX());
directions.push_back(-Vector3d::UnitX());
Vector3d direction = Vector3d(-1.0, 0.0, 1.0);
direction.normalize();
for (int i=0; i < 1.5*numInit1; i++)
directions.push_back(direction);
direction = Vector3d(0.0, -1.0, 2.0);
direction.normalize();
for (int i=0; i < 1.5*numInit1; i++)
directions.push_back(direction);
direction = Vector3d(-2.0, 1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit1; i++)
directions.push_back(direction);
direction = Vector3d(-2.0, -1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit1; i++)
directions.push_back(direction);
direction = Vector3d(0.0, 1.0, -2.0);
direction.normalize();
for (int i=0; i < 3*numInit1; i++)
directions.push_back(direction);
direction = Vector3d(0.0, -1.0, -2.0);
direction.normalize();
for (int i=0; i < 3*numInit1; i++)
directions.push_back(direction);
direction = Vector3d(2.0, 1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit1; i++)
directions.push_back(direction);
direction = Vector3d(2.0, -1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit1; i++)
directions.push_back(direction);
direction = Vector3d(0.0, 1.0, 2.0);
direction.normalize();
for (int i=0; i < 1.5*numInit1; i++)
directions.push_back(direction);
direction = Vector3d(0.0, -1.0, 2.0);
direction.normalize();
for (int i=0; i < 1.5*numInit1; i++)
directions.push_back(direction);
directions.push_back(-Vector3d::UnitY());
directions.push_back(-Vector3d::UnitY());
vertices.clear();
vertices.push_back(Vector3d::Zero());
for (int i=1; i < 16*numInit1 + 5; i++) {
Vector3d next_vertex;
next_vertex(0) = vertices[i-1](0) + directions[i-1](0) * lengths[i-1];
next_vertex(1) = vertices[i-1](1) + directions[i-1](1) * lengths[i-1];
next_vertex(2) = vertices[i-1](2) + directions[i-1](2) * lengths[i-1];
vertices.push_back(next_vertex);
}
Vector3d last_pos = Vector3d(10.0, -30.0, 0.0);
for (int i=0; i<vertices.size(); i++)
vertices[i] += -vertices.back() + last_pos;
Matrix3d start_rotation1 = (Matrix3d) AngleAxisd(M_PI, Vector3d::UnitZ());
Matrix3d end_rotation1 = (Matrix3d) AngleAxisd(M_PI/2.0, Vector3d::UnitZ());
ThreadConstrained* thread1 = new ThreadConstrained(vertices, angles, lengths, start_rotation1, this);
threads.push_back(thread1);
}
for (int thread_ind=0; thread_ind < threads.size(); thread_ind++) {
#ifndef ISOTROPIC
Matrix2d B = Matrix2d::Zero();
B(0,0) = 10.0;
B(1,1) = 1.0;
threads[thread_ind]->set_coeffs_normalized(B, 3.0, 1e-4);
#else
threads[thread_ind]->set_coeffs_normalized(1.0, 3.0, 1e-4);
#endif
}
}
void World::initThreadSingle()
{
int numInit = 12;
double first_length = 3.0; //FIRST_REST_LENGTH;
double second_length = SECOND_REST_LENGTH;
double middle_length = DEFAULT_REST_LENGTH;
vector<Vector3d> vertices;
vector<double> angles;
vector<double> lengths;
vector<Vector3d> directions;
for (int i=0; i < 2*numInit + 5; i++)
angles.push_back(0.0);
lengths.push_back(first_length);
lengths.push_back(second_length);
for (int i=0; i < 2*numInit; i++)
lengths.push_back(middle_length);
lengths.push_back(second_length);
lengths.push_back(first_length);
lengths.push_back(first_length);
directions.push_back(Vector3d::UnitX());
directions.push_back(Vector3d::UnitX());
Vector3d direction = Vector3d(1.0, -1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit; i++)
directions.push_back(direction);
direction = Vector3d(1.0, 1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit; i++)
directions.push_back(direction);
directions.push_back(Vector3d::UnitX());
directions.push_back(Vector3d::UnitX());
vertices.push_back(Vector3d::Zero());
for (int i=1; i < 2*numInit + 5; i++) {
Vector3d next_vertex;
next_vertex(0) = vertices[i-1](0) + directions[i-1](0) * lengths[i-1];
next_vertex(1) = vertices[i-1](1) + directions[i-1](1) * lengths[i-1];
next_vertex(2) = vertices[i-1](2) + directions[i-1](2) * lengths[i-1];
vertices.push_back(next_vertex);
}
Vector3d middle_desired_pos = Vector3d(0.0, -28.0, 0.0);
Vector3d middle_vertex_pos = vertices[vertices.size()/2];
for (int i=0; i<vertices.size(); i++)
vertices[i] += -middle_vertex_pos + middle_desired_pos;
Matrix3d start_rotation0 = Matrix3d::Identity();
Matrix3d end_rotation0 = Matrix3d::Identity();
ThreadConstrained* thread0 = new ThreadConstrained(vertices, angles, lengths, start_rotation0, end_rotation0, this);
threads.push_back(thread0);
for (int thread_ind=0; thread_ind < threads.size(); thread_ind++) {
#ifndef ISOTROPIC
Matrix2d B = Matrix2d::Zero();
B(0,0) = 10.0;
B(1,1) = 1.0;
threads[thread_ind]->set_coeffs_normalized(B, 3.0, 1e-4);
#else
threads[thread_ind]->set_coeffs_normalized(1.0, 3.0, 1e-4);
#endif
}
}
// Takes two edge-neighbor triangles, joins them into a quadrilateral,
// and then splits them the opposite way. This is used to repair degenerate
// triangles and to remove very small sliver triangles.
int trimesh_node_t::repair_triangles(trimesh_node_t* triangle_a, trimesh_node_t* triangle_b) {
int i,j, edge_a = -1, edge_b = -1, dummy;
vertex_t* quad_verticies[4];
trimesh_node_t* quad_neighbors[4];
trimesh_node_t* old_quad_neighbor[4];
trimesh_node_t* new_quad_neighbor[4];
vertex_t verticies_a[3];
vertex_t verticies_b[3];
vector_t normal_a, normal_b;
int vertex_a[3];
int vertex_b[3];
// Get the common edge
for (i=0; i<3; i++) {
if ((triangle_a->neighbors[i]) == triangle_b)
edge_a = i;
if ((triangle_b->neighbors[i]) == triangle_a)
edge_b = i;
}
// If these triangles are not edge neighbors, return without doing anything
if ((edge_a == -1) || (edge_b == -1))
return(1);
// The verticies for quadrilateral, in order, are:
// opposite vertex of a from edge
// next vertex of a
// opposite vertex of b from edge
// next vetex of a
vertex_a[0] = opposite_vertex_from_edge(edge_a);
vertex_a[1] = next_vertex(vertex_a[0]);
vertex_a[2] = next_vertex(vertex_a[1]);
vertex_b[0] = opposite_vertex_from_edge(edge_b);
vertex_b[1] = next_vertex(vertex_b[0]);
vertex_b[2] = next_vertex(vertex_b[1]);
quad_verticies[0] = triangle_a->verticies[vertex_a[0]];
quad_verticies[1] = triangle_a->verticies[vertex_a[1]];
quad_verticies[2] = triangle_b->verticies[vertex_b[0]];
quad_verticies[3] = triangle_b->verticies[vertex_b[1]];
// The edge neighbors for the quadrilateral come from calling
// get edge on the vertex number list...
quad_neighbors[0] = triangle_a->neighbors[get_edge_number(vertex_a[0], vertex_a[1])];
quad_neighbors[1] = triangle_b->neighbors[get_edge_number(vertex_b[2], vertex_b[0])];
quad_neighbors[2] = triangle_b->neighbors[get_edge_number(vertex_b[0], vertex_b[1])];
quad_neighbors[3] = triangle_a->neighbors[get_edge_number(vertex_a[2], vertex_a[0])];
// Test resultant triangles to make sure they both pass the ratio test, and their
// normal angle is not too different. If not, do not do this. (This catches the case of
// non-convex quadrilaterals)
verticies_a[0] = *quad_verticies[0];
verticies_a[1] = *quad_verticies[1];
verticies_a[2] = *quad_verticies[2];
verticies_b[0] = *quad_verticies[2];
verticies_b[1] = *quad_verticies[3];
verticies_b[2] = *quad_verticies[0];
if (ratio_test(verticies_a, &dummy) && ratio_test(verticies_b, &dummy)) {
normal_a = normalize(cross(sub(verticies_a[1],verticies_a[0]), sub(verticies_a[2], verticies_a[0])));
normal_b = normalize(cross(sub(verticies_b[1],verticies_b[0]), sub(verticies_b[2], verticies_b[0])));
// If triangles have the same normal to within 90 degrees...
if (dot(normal_a, normal_b) > 0) {
// Write them out.
old_quad_neighbor[0] = triangle_a;
old_quad_neighbor[1] = triangle_b;
old_quad_neighbor[2] = triangle_b;
old_quad_neighbor[3] = triangle_a;
// Now split the quadrilateral into two triangles, overwriting the old triangles
// Update Verticies
triangle_a->verticies[0] = quad_verticies[0];
triangle_a->verticies[1] = quad_verticies[1];
triangle_a->verticies[2] = quad_verticies[2];
triangle_b->verticies[0] = quad_verticies[2];
triangle_b->verticies[1] = quad_verticies[3];
triangle_b->verticies[2] = quad_verticies[0];
// Update Neighbors
triangle_a->neighbors[0] = quad_neighbors[0];
triangle_a->neighbors[1] = quad_neighbors[1];
triangle_a->neighbors[2] = triangle_b;
triangle_b->neighbors[0] = quad_neighbors[2];
triangle_b->neighbors[1] = quad_neighbors[3];
triangle_b->neighbors[2] = triangle_a;
// Update our neigbors as to their new neighbors
new_quad_neighbor[0] = triangle_a;
new_quad_neighbor[1] = triangle_a;
new_quad_neighbor[2] = triangle_b;
new_quad_neighbor[3] = triangle_b;
for (j=0; j<4; j++) {
for(i=0; i<3; i++) {
if (quad_neighbors[j]->neighbors[i] == old_quad_neighbor[j]) {
quad_neighbors[j]->neighbors[i] = new_quad_neighbor[j];
break;
}
}
}
return(0);
}
else {
return(1);
}
}
else {
return(1);
}
}
