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_);
}
