Vec3d GLGizmoRotate::mouse_position_in_local_plane(const Linef3& mouse_ray, const Selection& selection) const
{
double half_pi = 0.5 * (double)PI;
Transform3d m = Transform3d::Identity();
switch (m_axis)
{
case X:
{
m.rotate(Eigen::AngleAxisd(half_pi, Vec3d::UnitZ()));
m.rotate(Eigen::AngleAxisd(-half_pi, Vec3d::UnitY()));
break;
}
case Y:
{
m.rotate(Eigen::AngleAxisd(half_pi, Vec3d::UnitY()));
m.rotate(Eigen::AngleAxisd(half_pi, Vec3d::UnitZ()));
break;
}
default:
case Z:
{
// no rotation applied
break;
}
}
if (selection.is_single_volume() || selection.is_single_modifier() || selection.requires_local_axes())
m = m * selection.get_volume(*selection.get_volume_idxs().begin())->get_instance_transformation().get_matrix(true, false, true, true).inverse();
m.translate(-m_center);
return transform(mouse_ray, m).intersect_plane(0.0);
}
/// Computes the distance from another sphere primitive
double SpherePrimitive::calc_signed_dist(shared_ptr<const SpherePrimitive> s, Point3d& pthis, Point3d& ps) const
{
// get the transform from s to this
Transform3d T = Pose3d::calc_relative_pose(ps.pose, pthis.pose);
// compute the distance
double d = T.x.norm() - _radius - s->_radius;
// setup sphere centers in alternate frames
Point3d ps_c(0.0, 0.0, 0.0, ps.pose);
Point3d pthis_c(0.0, 0.0, 0.0, pthis.pose);
// setup closest points
pthis = T.transform_point(ps_c);
pthis.normalize();
ps = T.inverse_transform_point(pthis_c);
ps.normalize();
// scale closest points appropriately
if (d > 0.0)
{
pthis *= _radius;
ps *= s->_radius;
}
else
{
pthis *= _radius+d;
ps *= s->_radius+d;
}
return d;
}
/// Gets the distance from a sphere primitive
double PlanePrimitive::calc_signed_dist(shared_ptr<const SpherePrimitive> s, Point3d& pthis, Point3d& ps) const
{
const unsigned Y = 1;
assert(_poses.find(const_pointer_cast<Pose3d>(pthis.pose)) != _poses.end());
// compute the transform from the sphere to the plane
Transform3d T = Pose3d::calc_relative_pose(ps.pose, pthis.pose);
// transform the sphere center to the plane space
Point3d ps_c(0.0, 0.0, 0.0, ps.pose);
Point3d ps_c_this = T.transform_point(ps_c);
// get the lowest point on the sphere (toward the heightmap)
Vector3d vdir(0.0, -1.0*s->get_radius(), 0.0, pthis.pose);
// get the lowest point on the sphere
Point3d sphere_lowest = ps_c_this + vdir;
// setup the point on the plane
pthis = ps_c_this;
pthis[Y] = 0.0;
// get the height of the sphere center
ps = T.inverse_transform_point(sphere_lowest);
return sphere_lowest[Y];
}
/// Transforms the given triangle using the specified pose
Triangle Triangle::transform(const Triangle& t, const Transform3d& m)
{
Point3d a = m.transform_point(t.a);
Point3d b = m.transform_point(t.b);
Point3d c = m.transform_point(t.c);
return Triangle(a, b, c);
}
// setup simulator callback
void post_step_callback(Simulator* sim)
{
const unsigned X = 0, Y = 1, Z = 2;
// setup the box height
const double H = 1.0;
// get the bottoms of the box
Transform3d wTs = Pose3d::calc_relative_pose(box->get_pose(), GLOBAL);
Vector3d p1 = wTs.transform_point(Vector3d(-.5, -.5, -.5, box->get_pose()));
Vector3d p2 = wTs.transform_point(Vector3d(-.5, -.5, .5, box->get_pose()));
Vector3d p3 = wTs.transform_point(Vector3d(.5, -.5, -.5, box->get_pose()));
Vector3d p4 = wTs.transform_point(Vector3d(.5, -.5, .5, box->get_pose()));
// get the bottoms of the box in the global frame
shared_ptr<Pose3d> P1(new Pose3d), P2(new Pose3d), P3(new Pose3d), P4(new Pose3d);
P1->x = Origin3d(p1);
P2->x = Origin3d(p2);
P3->x = Origin3d(p3);
P4->x = Origin3d(p4);
// get the velocity of the box at the contact points
SVelocityd v = box->get_velocity();
Vector3d xd1 = Pose3d::calc_relative_pose(v.pose, P1).transform(v).get_linear();
Vector3d xd2 = Pose3d::calc_relative_pose(v.pose, P2).transform(v).get_linear();
Vector3d xd3 = Pose3d::calc_relative_pose(v.pose, P3).transform(v).get_linear();
Vector3d xd4 = Pose3d::calc_relative_pose(v.pose, P4).transform(v).get_linear();
/*
SVelocityd v = box->get_velocity();
Origin3d xd(v.get_linear());
Origin3d omega(v.get_angular());
Origin3d s(1.0, 0.0, 0.0);
Origin3d t(0.0, 1.0, 0.0);
Origin3d crosss = Origin3d::cross(-wTs.x, s);
Origin3d crosst = Origin3d::cross(-wTs.x, t);
*/
// output the sliding velocity at the contact
std::ofstream out("contactv.dat", std::ostream::app);
out << sim->current_time << " " << xd1[X] << " " << xd1[Y] << " " << xd2[X] << " " << xd2[Y] << " " << xd3[X] << " " << xd3[Y] << " " << xd4[X] << " " << xd4[Y] << std::endl;
// out << sim->current_time << " " << (s.dot(xd) + crosss.dot(omega)) << " " << (t.dot(xd) + crosst.dot(omega)) << std::endl;
// out << sim->current_time << " " << v[3] << " " << v[4] << " " << v[5] << " " << v[0] << " " << v[1] << " " << v[2] << std::endl;
out.close();
out.open("ke.dat", std::ostream::app);
out << sim->current_time << " " << box->calc_kinetic_energy() << std::endl;
out.close();
}
/// Tests intersection between a SSR and a SSL
bool BV::intersects(const SSR* A, const SSL* B, const Transform3d& aTb)
{
Point3d Bp1 = aTb.transform_point(B->p1);
Point3d Bp2 = aTb.transform_point(B->p2);
double dist = SSR::calc_dist(*A, LineSeg3(Bp1, Bp2));
return dist <= B->radius;
}
/**
* \param ATS the matrix transforming S's frame to A's frame
*/
bool BV::intersects(const AABB* A, const BoundingSphere* S, const Transform3d& ATS)
{
// create a new bounding sphere in O's frame
BoundingSphere s = *S;
s.center = ATS.transform_point(S->center);
return intersects(A, &s);
}
void rotateFrame(brics_3d::WorldModel* wm, unsigned int transformNodeId) {
Eigen::Vector3d axis(1,1,1);
axis.normalize();
Eigen::AngleAxis<double> rotation(rotationValue, axis);
//hen setting up an AngleAxis object, the axis vector must be normalized.
Transform3d transformation;
transformation = Eigen::Affine3d::Identity();
transformation.translate(Eigen::Vector3d(0.15,0,0));
transformation.rotate(rotation);
brics_3d::HomogeneousMatrix44::IHomogeneousMatrix44Ptr transform(new brics_3d::HomogeneousMatrix44(&transformation));
wm->scene.setTransform(transformNodeId, transform, brics_3d::rsg::TimeStamp(timer.getCurrentTime()));
rotationValue += 0.01;
}
// setup simulator callback
void post_step_callback(Simulator* s)
{
const unsigned X = 0, Y = 1, Z = 2;
// setup the sphere radius
const double R = 1.0;
// get the bottom of the sphere
Transform3d wTs = Pose3d::calc_relative_pose(sphere->get_pose(), GLOBAL);
shared_ptr<Pose3d> Pbot(new Pose3d);
Pbot->rpose = GLOBAL;
Pbot->x = wTs.x;
Pbot->x[Z] -= R;
// get the velocity of the sphere at the contact point
SVelocityd v = sphere->get_velocity();
Transform3d botTv = Pose3d::calc_relative_pose(v.pose, Pbot);
SVelocityd xd = botTv.transform(v);
Vector3d linear = xd.get_linear();
/*
SVelocityd v = sphere->get_velocity();
Origin3d xd(v.get_linear());
Origin3d omega(v.get_angular());
Origin3d s(1.0, 0.0, 0.0);
Origin3d t(0.0, 1.0, 0.0);
Origin3d crosss = Origin3d::cross(-wTs.x, s);
Origin3d crosst = Origin3d::cross(-wTs.x, t);
*/
// output the sliding velocity at the contact
std::ofstream out("contactv.dat", std::ostream::app);
out << sim->current_time << " " << linear[X] << " " << linear[Y] << " " << linear[Z] << std::endl;
// out << sim->current_time << " " << (s.dot(xd) + crosss.dot(omega)) << " " << (t.dot(xd) + crosst.dot(omega)) << std::endl;
// out << sim->current_time << " " << v[3] << " " << v[4] << " " << v[5] << " " << v[0] << " " << v[1] << " " << v[2] << std::endl;
out.close();
out.open("velocity.dat", std::ostream::app);
out << sim->current_time << " " << v[3] << " " << v[4] << " " << v[5] << " " << v[0] << " " << v[1] << " " << v[2] << std::endl;
out.close();
out.open("ke.dat", std::ostream::app);
out << sim->current_time << " " << sphere->calc_kinetic_energy() << std::endl;
out.close();
}
/**
* \param S the sphere-swept rectangle
* \param A the axis-aligned bounding box
* \param STA the transformation from A's frame to S's frame
*/
bool BV::intersects(const SSR* S, const AABB* A, const Transform3d& STA)
{
Transform3d ATS = STA.inverse();
SSR Sx;
AABB S_aabb;
S->transform(ATS, &Sx);
S_aabb.minp = Sx.get_lower_bounds();
S_aabb.maxp = Sx.get_upper_bounds();
return AABB::intersects(*A, S_aabb);
}
/// Transforms the BoundingSphere using the given transform
void BoundingSphere::transform(const Transform3d& T, BV* result) const
{
// get the BoundingSphere
BoundingSphere& s = *((BoundingSphere*) result);
// copy this
s = *this;
// transform the center
s.center = T.transform_point(center);
}
/**
* \param S the sphere-swept rectangle
* \param B the bounding sphere
* \param STB transformation from B's frame to S's frame
*/
bool BV::intersects(const SSR* S, const BoundingSphere* B, const Transform3d& STB)
{
// transform the center of the bounding sphere
Point3d xc = STB.transform_point(B->center);
// determine the distance between S and xformed center of the bounding sphere
double dist = SSR::calc_dist(*S, xc);
// check whether the distance is within the radius of the bounding sphere
return dist - B->radius <= (double) 0.0;
}
/// Gets the distance from a polyhedral primitive
double PlanePrimitive::calc_signed_dist(shared_ptr<const PolyhedralPrimitive> b, Point3d& pthis, Point3d& pb) const
{
const unsigned Y = 1;
assert(_poses.find(const_pointer_cast<Pose3d>(pthis.pose)) != _poses.end());
// make sure that the primitive is convex
assert(b->is_convex());
// compute the transform from the primitive to the plane
Transform3d T = Pose3d::calc_relative_pose(pb.pose, pthis.pose);
// setup initial minimum distance
double min_dist = std::numeric_limits<double>::max();
// get the polyhedron vertices
shared_ptr<PolyhedralPrimitive> bnc = const_pointer_cast<PolyhedralPrimitive>(b);
vector<Point3d> verts;
bnc->get_vertices(pb.pose, verts);
// find which vertex is closest in the plane space
for (unsigned i=0; i< verts.size(); i++)
{
// get the polyhedron vertex in the plane space
Point3d p_vert = T.transform_point(verts[i]);
// get the vertex height
if (p_vert[Y] < min_dist)
{
min_dist = p_vert[Y];
pb = verts[i];
pthis = p_vert;
}
}
// closest point on plane is just the closest point on the plane, projected
// to the plane
pthis[Y] = 0.0;
return min_dist;
}
void HomogeneousMatrixTest::testInverse() {
HomogeneousMatrix44 pureTranslation(1,0,0, 0,1,0, 0,0,1, 1,2,3);
// cout << "pureTranslation:" << endl << pureTranslation << endl;
pureTranslation.inverse();
// cout << "inverse of pureTranslation:" << endl << pureTranslation << endl;
matrixPtr = pureTranslation.getRawData();
CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, matrixPtr[12], maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-2.0, matrixPtr[13], maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-3.0, matrixPtr[14], maxTolerance);
HomogeneousMatrix44 pureTranslation2(1,0,0, 0,1,0, 0,0,1, 3,4,5);
HomogeneousMatrix44 pureTranslation3(1,0,0, 0,1,0, 0,0,1, 3,4,5);
pureTranslation3.inverse();
IHomogeneousMatrix44* result = new HomogeneousMatrix44();
result = pureTranslation2 * pureTranslation3;
CPPUNIT_ASSERT(result->isIdentity() == true);
/* now translation and rotation */
AngleAxis<double> rotation(M_PI_2/4.0, Vector3d(1,0,0));
Transform3d transformation;
transformation = rotation;
transformation.translate(Vector3d(5,6,99.9));
IHomogeneousMatrix44* someTransform = new HomogeneousMatrix44(&transformation);
IHomogeneousMatrix44* someOtherTransform = new HomogeneousMatrix44();
*someOtherTransform = *someTransform;
someTransform->inverse();
// cout << "someTransform:" << endl << *someTransform << endl;
result = (*someTransform) * (*someOtherTransform);
// cout << "result:" << endl << *result << endl;
CPPUNIT_ASSERT(result->isIdentity() == true);
}
/**
* \param aTb the relative transformation from b to a
*/
bool BV::intersects(const BV* a, const BV* b, const Transform3d& aTb)
{
// look for dummy type
if (dynamic_cast<const DummyBV*>(a) || dynamic_cast<const DummyBV*>(b))
return true;
// look for OBB type
if (dynamic_cast<const OBB*>(a))
{
// OBB / OBB intersection
if (dynamic_cast<const OBB*>(b))
return OBB::intersects(*((const OBB*) a), *((const OBB*) b), aTb);
// OBB / SSR intersection
if (dynamic_cast<const SSR*>(b))
return intersects((const OBB*) a, (const SSR*) b, aTb);
// OBB / SSL intersection
if (dynamic_cast<const SSL*>(b))
return intersects((const OBB*) a, (const SSL*) b, aTb);
// OBB / bounding sphere intersection
if (dynamic_cast<const BoundingSphere*>(b))
return intersects((const OBB*) a, (const BoundingSphere*) b, aTb);
// OBB / AABB intersection
if (dynamic_cast<const AABB*>(b))
return intersects((const OBB*) a, (const AABB*) b, aTb);
}
// look for SSR type
else if (dynamic_cast<const SSR*>(a))
{
// SSR / SSR intersection
if (dynamic_cast<const SSR*>(b))
return SSR::intersects(*((const SSR*) a), *((const SSR*) b), aTb);
// SSR / SSL intersection
if (dynamic_cast<const SSL*>(b))
return intersects((const SSR*) a, (const SSL*) b, aTb);
// SSR / bounding sphere intersection
if (dynamic_cast<const BoundingSphere*>(b))
return intersects((const SSR*) a, (const BoundingSphere*) b, aTb);
// OBB / SSR intersection
if (dynamic_cast<const OBB*>(b))
return intersects((const OBB*) b, (const SSR*) a, aTb.inverse());
// AABB / SSR intersection
if (dynamic_cast<const AABB*>(b))
return intersects((const SSR*) a, (const AABB*) b, aTb);
}
else if (dynamic_cast<const SSL*>(a))
{
// OBB / SSL intersection
if (dynamic_cast<const OBB*>(b))
return intersects((const OBB*) b, (const SSL*) a, aTb.inverse());
// SSR / SSL intersection
if (dynamic_cast<const SSR*>(b))
return intersects((const SSR*) b, (const SSL*) a, aTb.inverse());
// SSL / SSL intersection
if (dynamic_cast<const SSL*>(b))
return SSL::intersects(*((const SSL*) a), *((const SSL*) b), aTb);
// SSL / BoundingSphere intersection
if (dynamic_cast<const BoundingSphere*>(b))
return intersects((const SSL*) a, (const BoundingSphere*) b, aTb.inverse());
// SSL / AABB intersection
if (dynamic_cast<const AABB*>(b))
return intersects((const AABB*) b, (const SSL*) a, aTb.inverse());
}
else if (dynamic_cast<const BoundingSphere*>(a))
{
// OBB / bounding sphere intersection
if (dynamic_cast<const OBB*>(b))
return intersects((const OBB*) b, (const BoundingSphere*) a, aTb.inverse());
// SSR / bounding sphere intersection
if (dynamic_cast<const SSR*>(b))
return intersects((const SSR*) b, (const BoundingSphere*) a, aTb.inverse());
// SSL / bounding sphere intersection
if (dynamic_cast<const SSL*>(b))
return intersects((const SSL*) b, (const BoundingSphere*) a, aTb.inverse());
// bounding sphere / bounding sphere intersection
if (dynamic_cast<const BoundingSphere*>(b))
return BoundingSphere::intersects(*((const BoundingSphere*) a), *((const BoundingSphere*) b), aTb);
// AABB / bounding sphere intersection
if (dynamic_cast<const AABB*>(b))
return intersects((const AABB*) b, (const BoundingSphere*) a, aTb.inverse());
}
else if (dynamic_cast<const AABB*>(a))
{
// OBB / AABB intersection
if (dynamic_cast<const OBB*>(b))
return intersects((const OBB*) b, (const AABB*) a, aTb.inverse());
// SSR / AABB intersection
if (dynamic_cast<const SSR*>(b))
return intersects((const SSR*) b, (const AABB*) a, aTb.inverse());
// SSL / AABB intersection
if (dynamic_cast<const SSL*>(b))
return intersects((const AABB*) a, (const SSL*) b, aTb);
// BoundingSphere / AABB intersection
if (dynamic_cast<const BoundingSphere*>(b))
return intersects((const AABB*) a, (const BoundingSphere*) b, aTb);
// AABB / AABB intersection
if (dynamic_cast<const AABB*>(b))
return AABB::intersects(*((const AABB*) a), *((const AABB*) b), aTb);
}
// shouldn't be here
assert(false);
return true;
}
void RoiManager::execute() {
LOG(INFO) << "RoiManager: Adding a new ROI.";
if (inputDataIds.size() < 1) {
LOG(WARNING) << "RoiManager: Not enough input IDs.";
return;
}
brics_3d::rsg::Id rootId = inputDataIds[0];
Eigen::AngleAxis<double> rotation(roiPitch, Eigen::Vector3d(1,0,0));
Transform3d transformation;
transformation = Eigen::Affine3d::Identity();
transformation.translate(Eigen::Vector3d(roiCenterX,roiCenterY,roiCenterZ));
transformation.rotate(rotation);
brics_3d::HomogeneousMatrix44::IHomogeneousMatrix44Ptr transform(new brics_3d::HomogeneousMatrix44(&transformation));
brics_3d::rsg::Id tfBox1Id = 0;
brics_3d::rsg::Id Box1Id = 0;
vector<brics_3d::rsg::Attribute> tmpAttributes;
tmpAttributes.clear();
tmpAttributes.push_back(Attribute("name","roi_box_tf"));
wm->scene.addTransformNode(rootId, tfBox1Id, tmpAttributes, transform, brics_3d::rsg::TimeStamp(timer.getCurrentTime()));
brics_3d::rsg::Box::BoxPtr box1(new brics_3d::rsg::Box(roiBoxSizeX, roiBoxSizeY, roiBoxSizeZ));
tmpAttributes.clear();
tmpAttributes.push_back(Attribute("name","roi_box"));
// tmpAttributes.push_back(Attribute("debugInfo","no_visualization"));
tmpAttributes.push_back(Attribute("rsgInfo","non_shared"));
wm->scene.addGeometricNode(tfBox1Id, Box1Id, tmpAttributes, box1, brics_3d::rsg::TimeStamp(timer.getCurrentTime()));
LOG(DEBUG) << "ROI Box added with ID " << Box1Id;
/* Here comes a basic robot skeleton */
brics_3d::rsg::Id tfWorldToRobotId = 0;
tmpAttributes.clear();
tmpAttributes.push_back(Attribute("name","world_to_robot_tf"));
brics_3d::HomogeneousMatrix44::IHomogeneousMatrix44Ptr initialWorldToRobotTransform(new brics_3d::HomogeneousMatrix44());
wm->scene.addTransformNode(wm->scene.getRootId(), tfWorldToRobotId, tmpAttributes, initialWorldToRobotTransform, brics_3d::rsg::TimeStamp(timer.getCurrentTime()));
brics_3d::rsg::Id tfRobotToSensorId = 0;
tmpAttributes.clear();
tmpAttributes.push_back(Attribute("name","robot_to_sensor_tf"));
brics_3d::HomogeneousMatrix44::IHomogeneousMatrix44Ptr initialRobotToSensorTransform(new brics_3d::HomogeneousMatrix44());
LOG(DEBUG) << "current times stamp for frame = "<< timer.getCurrentTime();
wm->scene.addTransformNode(tfWorldToRobotId, tfRobotToSensorId, tmpAttributes, initialRobotToSensorTransform, brics_3d::rsg::TimeStamp(timer.getCurrentTime()));
/* We will add a hook for the processing data */
brics_3d::rsg::Id sensorGroupId;
tmpAttributes.clear();
tmpAttributes.push_back(Attribute("name","sensor"));
wm->scene.addGroup(tfRobotToSensorId, sensorGroupId, tmpAttributes);
LOG(DEBUG) << "Sensor group added with ID " << sensorGroupId;
/* We will add a hook for the scene objects */
brics_3d::rsg::Id sceneObjectsGroupId;
tmpAttributes.clear();
tmpAttributes.push_back(Attribute("name","sceneObjects"));
wm->scene.addGroup(wm->scene.getRootId(), sceneObjectsGroupId, tmpAttributes);
LOG(DEBUG) << "Scene objects group added with ID " << sensorGroupId;
}
