int IntersectTriangleLine (const Point<3> ** tri, const Point<3> ** line) { Vec3d vl(*line[0], *line[1]); Vec3d vt1(*tri[0], *tri[1]); Vec3d vt2(*tri[0], *tri[2]); Vec3d vrs(*tri[0], *line[0]); static DenseMatrix a(3), ainv(3); static Vector rs(3), lami(3); int i; /* (*testout) << "Tri-Line inters: " << endl << "tri = " << *tri[0] << ", " << *tri[1] << ", " << *tri[2] << endl << "line = " << *line[0] << ", " << *line[1] << endl; */ for (i = 1; i <= 3; i++) { a.Elem(i, 1) = -vl.X(i); a.Elem(i, 2) = vt1.X(i); a.Elem(i, 3) = vt2.X(i); rs.Elem(i) = vrs.X(i); } double det = a.Det(); double arel = vl.Length() * vt1.Length() * vt2.Length(); /* double amax = 0; for (i = 1; i <= 9; i++) if (fabs (a.Get(i)) > amax) amax = fabs(a.Get(i)); */ // new !!!! if (fabs (det) <= 1e-10 * arel) { #ifdef DEVELOP // line parallel to triangle ! // cout << "ERROR: IntersectTriangleLine degenerated" << endl; // (*testout) << "WARNING: IntersectTriangleLine degenerated\n"; /* (*testout) << "lin-tri intersection: " << endl << "line = " << *line[0] << " - " << *line[1] << endl << "tri = " << *tri[0] << " - " << *tri[1] << " - " << *tri[2] << endl << "lami = " << lami << endl << "pc = " << ( *line[0] + lami.Get(1) * vl ) << endl << " = " << ( *tri[0] + lami.Get(2) * vt1 + lami.Get(3) * vt2) << endl << " a = " << a << endl << " ainv = " << ainv << endl << " det(a) = " << det << endl << " rs = " << rs << endl; */ #endif return 0; } CalcInverse (a, ainv); ainv.Mult (rs, lami); // (*testout) << "lami = " << lami << endl; double eps = 1e-6; if ( (lami.Get(1) >= -eps && lami.Get(1) <= 1+eps && lami.Get(2) >= -eps && lami.Get(3) >= -eps && lami.Get(2) + lami.Get(3) <= 1+eps) && ! (lami.Get(1) >= eps && lami.Get(1) <= 1-eps && lami.Get(2) >= eps && lami.Get(3) >= eps && lami.Get(2) + lami.Get(3) <= 1-eps) ) { #ifdef DEVELOP // cout << "WARNING: IntersectTriangleLine degenerated" << endl; (*testout) << "WARNING: IntersectTriangleLine numerical inexact" << endl; (*testout) << "lin-tri intersection: " << endl << "line = " << *line[0] << " - " << *line[1] << endl << "tri = " << *tri[0] << " - " << *tri[1] << " - " << *tri[2] << endl << "lami = " << lami << endl << "pc = " << ( *line[0] + lami.Get(1) * vl ) << endl << " = " << ( *tri[0] + lami.Get(2) * vt1 + lami.Get(3) * vt2) << endl << " a = " << a << endl << " ainv = " << ainv << endl << " det(a) = " << det << endl << " rs = " << rs << endl; #endif } if (lami.Get(1) >= 0 && lami.Get(1) <= 1 && lami.Get(2) >= 0 && lami.Get(3) >= 0 && lami.Get(2) + lami.Get(3) <= 1) { return 1; } return 0; }
Mesh* createMeshFromVertices(const EigenSTL::vector_Vector3d &source) { if (source.size() < 3) return NULL; if (source.size() % 3 != 0) logError("The number of vertices to construct a mesh from is not divisible by 3. Probably constructed triangles will not make sense."); std::set<detail::LocalVertexType, detail::ltLocalVertexValue> vertices; std::vector<unsigned int> triangles; unsigned int n = source.size() / 3; for (unsigned int i = 0 ; i < n ; ++i) { // check if we have new vertices unsigned int i3 = i * 3; detail::LocalVertexType vt1(source[i3]); std::set<detail::LocalVertexType, detail::ltLocalVertexValue>::iterator p1 = vertices.find(vt1); if (p1 == vertices.end()) { vt1.index = vertices.size(); vertices.insert(vt1); } else vt1.index = p1->index; triangles.push_back(vt1.index); detail::LocalVertexType vt2(source[++i3]); std::set<detail::LocalVertexType, detail::ltLocalVertexValue>::iterator p2 = vertices.find(vt2); if (p2 == vertices.end()) { vt2.index = vertices.size(); vertices.insert(vt2); } else vt2.index = p2->index; triangles.push_back(vt2.index); detail::LocalVertexType vt3(source[++i3]); std::set<detail::LocalVertexType, detail::ltLocalVertexValue>::iterator p3 = vertices.find(vt3); if (p3 == vertices.end()) { vt3.index = vertices.size(); vertices.insert(vt3); } else vt3.index = p3->index; triangles.push_back(vt3.index); } // sort our vertices std::vector<detail::LocalVertexType> vt; vt.insert(vt.end(), vertices.begin(), vertices.end()); std::sort(vt.begin(), vt.end(), detail::ltLocalVertexIndex()); // copy the data to a mesh structure unsigned int nt = triangles.size() / 3; Mesh *mesh = new Mesh(vt.size(), nt); for (unsigned int i = 0 ; i < vt.size() ; ++i) { unsigned int i3 = i * 3; mesh->vertices[i3 ] = vt[i].x; mesh->vertices[i3 + 1] = vt[i].y; mesh->vertices[i3 + 2] = vt[i].z; } std::copy(triangles.begin(), triangles.end(), mesh->triangles); mesh->computeNormals(); return mesh; }
void DiffDrivePlugin::publish_odometry() { ros::Time current_time = ros::Time::now(); /*std::string odom_frame = tf::resolve(tf_prefix_, "odom"); std::string base_footprint_frame = tf::resolve(tf_prefix_, "base_footprint"); // getting data for base_footprint to odom transform math::Pose pose = this->parent->GetWorldPose(); tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w); tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z); tf::Transform base_footprint_to_odom(qt, vt); transform_broadcaster_->sendTransform(tf::StampedTransform(base_footprint_to_odom, current_time, odom_frame, base_footprint_frame));*/ std::string odom_frame = tf::resolve(tf_prefix_, "odom"); std::string base_footprint_frame = tf::resolve(tf_prefix_, "depth"); // getting data for base_footprint to odom transform math::Pose pose = this->parent->GetWorldPose(); tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w); tf::Vector3 vt(pose.pos.x + pose.pos.x * this->error_, pose.pos.y + pose.pos.y * this->error_, 0); tf::Transform depth_to_odom(qt, vt); transform_broadcaster_->sendTransform(tf::StampedTransform(depth_to_odom, current_time, odom_frame, "depth")); qt.setRPY(0, 0, 0);; tf::Vector3 vt2(0, 0, pose.pos.z); tf::Transform base_footprint_to_depth(qt, vt2); transform_broadcaster_->sendTransform(tf::StampedTransform(base_footprint_to_depth, current_time, "depth", "base_footprint")); // publish odom topic odom_.pose.pose.position.x = pose.pos.x + pose.pos.x * this->error_; odom_.pose.pose.position.y = pose.pos.y + pose.pos.y * this->error_; odom_.pose.pose.position.z = pose.pos.z; odom_.pose.pose.orientation.x = pose.rot.x; odom_.pose.pose.orientation.y = pose.rot.y; odom_.pose.pose.orientation.z = pose.rot.z; odom_.pose.pose.orientation.w = pose.rot.w; math::Vector3 linear = this->parent->GetWorldLinearVel(); odom_.twist.twist.linear.x = linear.x; odom_.twist.twist.linear.y = linear.y; odom_.twist.twist.linear.y = linear.z; odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().z; odom_.header.stamp = current_time; odom_.header.frame_id = odom_frame; odom_.child_frame_id = base_footprint_frame; pub_.publish(odom_); }