DANI_INLINE void EpsTrimesh::set_t_labels(const std::vector<int> &labels) { assert (labels.size() == num_triangles()); for (uint tid = 0; tid < num_triangles(); tid++) { set_t_label(tid, labels.at(tid)); } }
void Physics::step( real_t dt ) { // step the world forward by dt. Need to detect collisions, apply // forces, and integrate positions and orientations. for(size_t i=0; i<num_spheres(); i++){ //Update Spheres boundbox Vector3 upper = spheres[i]->position + Vector3(spheres[i]->radius, spheres[i]->radius, spheres[i]->radius); Vector3 lower = spheres[i]->position - Vector3(spheres[i]->radius, spheres[i]->radius, spheres[i]->radius); spheres[i]->bound = BoundBox(lower, upper); for(size_t j=0; j<num_spheres(); j++){ if(i!=j){ collides(*spheres[i], *spheres[j], collision_damping); } } for(size_t j=0; j<num_planes(); j++){ collides(*spheres[i], *planes[j], collision_damping); } for(size_t j=0; j<num_triangles(); j++){ collides(*spheres[i], *triangles[j], collision_damping); } for(size_t j=0; j<num_models(); j++){ collides(*spheres[i], *models[j], collision_damping); } //RK4 Position & Orientation rk4_update(*spheres[i], dt); spheres[i]->clear_force(); } }
float BVHNode::computeSubtreeSAHCost(const BVHParams& p, float probability) const { float SAH = probability * p.cost(num_children(), num_triangles()); for(int i = 0; i < num_children(); i++) { BVHNode *child = get_child(i); SAH += child->computeSubtreeSAHCost(p, probability * child->bounds.safe_area()/bounds.safe_area()); } return SAH; }
DANI_INLINE void EpsTrimesh::set_epsilon_triangle_label() { std::vector<double> field; //std::map<float, std::pair<int, float *>> colormap; for (uint tid = 0; tid < num_triangles(); tid++) { field.push_back(t_epsilons.at(tid).radius); } set_t_labels (field); }
void Physics::detect_collisions() { for (size_t i = 0; i < num_spheres(); i++) { for (size_t j = 0; j < num_spheres(); j++) { if (i != j) { collides(*(spheres[i]), *(spheres[j]), collision_damping); } } for (size_t j = 0; j < num_planes(); j++) { collides(*(spheres[i]), *(planes[j]), collision_damping); } for (size_t j = 0; j < num_triangles(); j++) { collides(*(spheres[i]), *(triangles[j]), collision_damping); } } }
void Physics::step( real_t dt ) { // Check collision between every sphere and other objects. for (size_t i = 0; i < num_spheres(); i++) { for (size_t j = 0; j < num_planes(); j++) collides(*spheres[i], *planes[j], collision_damping); for (size_t j = 0; j < num_triangles(); j++) collides(*spheres[i], *triangles[j], collision_damping); for (size_t j = i + 1; j < num_spheres(); j++) { collides(*spheres[i], *spheres[j], collision_damping); } } std::vector<State> states(num_spheres()); for (size_t i = 0; i < num_spheres(); i++) { states[i].position = spheres[i]->position; states[i].velocity = spheres[i]->velocity; states[i].angular_velocity = spheres[i]->angular_velocity; // Step dt using rk4 integral. rk4_integrate(states[i], spheres[i], dt); } // Update every sphere's state after rk4 integral. for (size_t i = 0; i < num_spheres(); i++) { spheres[i]->position = states[i].position; spheres[i]->velocity = states[i].velocity; spheres[i]->angular_velocity = states[i].angular_velocity; spheres[i]->update_orientation(states[i].angular_position); spheres[i]->sphere->orientation = spheres[i]->orientation; spheres[i]->sphere->position = states[i].position; // Update the offset vector of this sphere in every spring it is attached to. for (size_t j = 0; j < spheres[i]->spring_ptrs.size(); j++) { spheres[i]->spring_ptrs[j]->update_offset(spheres[i], states[i].angular_position); } } }
/** Return the iterator which corresponds to the past-the-last triangle. */ TriangleIterator triangle_end() const { return {this, num_triangles()}; }
/** * @brief EpsTrimesh::split_on_sphere_center_and_tangent * @param tid */ DANI_INLINE void EpsTrimesh::split_on_sphere_tangent(const uint tid) { Sphere *sphere = &t_epsilons.at(tid); if (sphere->tangent_pos == TANGENT_UNKNOWN) return; if (sphere->radius == 0.0) return; if (sphere->center_pos != CENTERED_ON_VERTEX) assert(false); uint vid = UINT_MAX; // center if (sphere->tangent_pos == TANGENT_ON_EDGE) { std::cout << ">>>>Sphere associated to triangle " << tid << " is tangent on edge .. splitting .. " << std::endl; vid = split_edge(sphere->tangent_id, sphere->tangent); for (uint tt : adj_vtx2tri(vid)) t_epsilons.at(tt).center_tri.first = tt; } else if (sphere->tangent_pos == TANGENT_ON_TRIANGLE) { std::cout << ">>>>Sphere associated to triangle " << tid << " is tangent on triangle .. splitting .. " << std::endl; vid = split_triangle(sphere->tangent_tri.first, sphere->tangent); for (uint tt : adj_vtx2tri(vid)) t_epsilons.at(tt).center_tri.first = tt; } else if (sphere->tangent_pos == TANGENT_ON_VERTEX) { std::cout << ">>>>Sphere associated to triangle " << tid << " is tangent on vertex. " << std::endl; } // Recursive if (vid != UINT_MAX) { for (uint tt=0; tt < num_triangles(); tt++) { if (t_epsilons.at(tt).tangent_pos == TANGENT_ON_EDGE && std::find(adj_vtx2edg(vid).begin(), adj_vtx2edg(vid).end(), t_epsilons.at(tt).tangent_id) != adj_vtx2edg(vid).end()) split_on_sphere_tangent(tt); else if (t_epsilons.at(tt).tangent_pos == TANGENT_ON_TRIANGLE && std::find(adj_vtx2tri(vid).begin(), adj_vtx2tri(vid).end(), t_epsilons.at(tt).tangent_tri.first) != adj_vtx2tri(vid).end()) split_on_sphere_tangent(tt); } } return; }
DANI_INLINE void EpsTrimesh::set_t_labels(const std::vector<double> labels_sdf) { #ifdef CINOLIB assert (labels_sdf.size() == num_triangles()); double min_value = FLT_MAX; std::vector<float> labels ;//= labels_sdf; for (uint tid = 0; tid < num_triangles(); tid++) { labels.push_back(labels_sdf.at(tid)); } ScalarField values (labels); values.normalize_in_01(); for (uint tid = 0; tid < num_triangles(); tid++) { double value = values[tid]; //.at(tid); if (value < min_value) min_value = labels.at(tid); } std::cout << "Minimum value from double to integer..." << std::endl; int factor = 1; int int_value = (int) min_value; double delta = 0.0; // used to handle minimum values equal to 0 if (min_value == 0.0) { delta = 1e-5; min_value += delta; } while (int_value <= 10) { factor *= 10; min_value = (min_value * factor); int_value = (int) min_value; std::cout << min_value << " -- " << int_value << std::endl; } //(std::cout << std::endl << " Min value " << min_value << " -- integer : " << int_value << std::endl; std::cout << "Associating triangle labels ..." << std::endl; for (uint tid = 0; tid < num_triangles(); tid++) { double value = values[tid] ; //int_labels.push_back( (int) (value *factor)); set_t_label(tid, (int) (value *10000)); } //set_t_labels (int_labels); #else std::cerr << "[ERROR] Cinolib required to run this operation. " << std::endl; exit(1); #endif }
/** * @brief EpsTrimesh::split_triangle * @param tid * @param point * @return */ DANI_INLINE uint EpsTrimesh::split_triangle(const uint tid, const vec3d point, std::vector<double> &radius, std::vector<vec3d> ¢ers, std::vector<vec3d> &antipodeans) { #ifdef CINOLIB uint nt = num_triangles(); uint vid = add_vertex(point); double r = radius.at(tid); vec3d a = antipodeans.at(tid); vec3d c = centers.at(tid); uint v0 = triangle_vertex_id(tid, 0); uint v1 = triangle_vertex_id(tid, 1); uint v2 = triangle_vertex_id(tid, 2); uint t0 = add_triangle(vid, v2, v0); uint t1 = add_triangle(vid, v0, v1); uint t2 = add_triangle(vid, v1, v2); radius.resize(num_triangles()); centers.resize(num_triangles()); antipodeans.resize(num_triangles()); radius.at(t0) = FLT_MAX; radius.at(t1) = FLT_MAX; radius.at(t2) = FLT_MAX; centers.at(t0) = {FLT_MAX}; centers.at(t1) = {FLT_MAX}; centers.at(t2) = {FLT_MAX}; antipodeans.at(t0) = {FLT_MAX}; antipodeans.at(t1) = {FLT_MAX}; antipodeans.at(t2) = {FLT_MAX}; tris.at(tid*3+0) = INT_MAX; tris.at(tid*3+1) = INT_MAX; tris.at(tid*3+2) = INT_MAX; if (tid == num_triangles()-1) { tris.resize(tris.size()-3); t_norm.resize(t_norm.size()-3); t_label.resize(t_label.size()-1); tri2edg.pop_back(); tri2tri.pop_back(); radius.pop_back(); antipodeans.pop_back(); centers.pop_back(); } else { remove_unreferenced_triangle(tid); std::swap(radius.at(radius.size()-1), radius.at(tid)); radius.pop_back(); std::swap(antipodeans.at(antipodeans.size()-1), antipodeans.at(tid)); antipodeans.pop_back(); std::swap(centers.at(centers.size()-1), centers.at(tid)); centers.pop_back(); } update_adjacency(); assert (num_triangles() == nt+2); assert (radius.size() == nt+2); assert (centers.size() == nt+2); assert (antipodeans.size() == nt+2); return vid; #else std::cerr << "[ERROR] Cinolib required to run this operation." << std::endl; exit(1); #endif }
DANI_INLINE int EpsTrimesh::add_triangle(const int vid0, const int vid1, const int vid2, const int scalar) { assert(vid0 >= 0 && vid0 < num_vertices()); assert(vid1 >= 0 && vid1 < num_vertices()); assert(vid2 >= 0 && vid2 < num_vertices()); int tid = num_triangles(); // tris.push_back(vid0); tris.push_back(vid1); tris.push_back(vid2); // t_label.push_back(scalar); // tri2edg.push_back(std::vector<int>()); tri2tri.push_back(std::vector<int>()); // vtx2tri.at(vid0).push_back(tid); vtx2tri.at(vid1).push_back(tid); vtx2tri.at(vid2).push_back(tid); // ipair new_e[3] = { unique_pair(vid0, vid1), unique_pair(vid1, vid2), unique_pair(vid2, vid0) }; int new_eid[3] = { -1, -1, -1 }; for(int eid=0; eid<num_edges(); ++eid) { ipair e = unique_pair(edge_vertex_id(eid, 0), edge_vertex_id(eid, 1)); for(int i=0; i<3; ++i) if (e == new_e[i]) new_eid[i] = eid; } // for(int i=0; i<3; ++i) { if (new_eid[i] == -1) { new_eid[i] = num_edges(); edges.push_back(new_e[i].first); edges.push_back(new_e[i].second); edg2tri.push_back(std::vector<int>()); vtx2edg.at(new_e[i].first).push_back(num_edges()-1); vtx2edg.at(new_e[i].second).push_back(num_edges()-1); vtx2vtx.at(new_e[i].first).push_back(new_e[i].second); vtx2vtx.at(new_e[i].second).push_back(new_e[i].first); } // for(int nbr : edg2tri.at(new_eid[i])) { tri2tri.at(nbr).push_back(tid); tri2tri.at(tid).push_back(nbr); } edg2tri.at(new_eid[i]).push_back(tid); tri2edg.at(tid).push_back(new_eid[i]); } // t_norm.push_back(0); //tnx t_norm.push_back(0); //tny t_norm.push_back(0); //tnz #ifdef CINOLIB // update_t_normal(tid); update_v_normal(vid0); update_v_normal(vid1); update_v_normal(vid2); #else std::cerr << "[WARNING] Cinolib required to update triangle and vertex normals." << std::endl; #endif return tid; }