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

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

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

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


}

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;

}