TEST_F(GeometryFixture, TranslationTransformations)
{
  double tol = 1.0E-12;
  Vector3d trans(1,1,1);
  Point3d point1(1,0,0);
  Transformation T;
  Point3d temp;

  // identity transformation
  temp = T*point1;
  EXPECT_NEAR(1.0, temp.x(), tol);
  EXPECT_NEAR(0.0, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // move by 1, 1, 1
  T = Transformation::translation(trans);
  temp = T*point1;
  EXPECT_NEAR(2.0, temp.x(), tol);
  EXPECT_NEAR(1.0, temp.y(), tol);
  EXPECT_NEAR(1.0, temp.z(), tol);

  // move by -1, -1, -1
  temp = T.inverse()*point1;
  EXPECT_NEAR(0.0, temp.x(), tol);
  EXPECT_NEAR(-1.0, temp.y(), tol);
  EXPECT_NEAR(-1.0, temp.z(), tol);

  // identity transformation
  temp = T.inverse()*T*point1;
  EXPECT_NEAR(1.0, temp.x(), tol);
  EXPECT_NEAR(0.0, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // identity transformation
  temp = T*T.inverse()*point1;
  EXPECT_NEAR(1.0, temp.x(), tol);
  EXPECT_NEAR(0.0, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);
}

void ExportFileFunctions::loadYRadiusValuesForCurveRaw (const DocumentModelCoords &modelCoords,
                                                        const MainWindowModel &modelMainWindow,
                                                        const Points &points,
                                                        const ExportValuesXOrY &xThetaValues,
                                                        const Transformation &transformation,
                                                        QVector<QString*> &yRadiusValues) const
{
  LOG4CPP_INFO_S ((*mainCat)) << "ExportFileFunctions::loadYRadiusValuesForCurveRaw";

  FormatCoordsUnits format;

  // Since the curve points may be a subset of xThetaValues (in which case the non-applicable xThetaValues will have
  // blanks for the yRadiusValues), we iterate over the smaller set
  for (int pt = 0; pt < points.count(); pt++) {

    const Point &point = points.at (pt);

    QPointF posGraph;
    transformation.transformScreenToRawGraph (point.posScreen(),
                                              posGraph);

    // Find the closest point in xThetaValues. This is probably an N-squared algorithm, which is less than optimial,
    // but the delay should be insignificant with normal-sized export files
    double closestSeparation = 0.0;
    int rowClosest = 0;
    for (int row = 0; row < xThetaValues.count(); row++) {

      double xThetaValue = xThetaValues.at (row);

      double separation = qAbs (posGraph.x() - xThetaValue);

      if ((row == 0) ||
          (separation < closestSeparation)) {

        closestSeparation = separation;
        rowClosest = row;

      }
    }

    // Save y/radius value for this row into yRadiusValues, after appropriate formatting
    QString dummyXThetaOut;
    format.unformattedToFormatted (posGraph.x(),
                                   posGraph.y(),
                                   modelCoords,
                                   modelMainWindow,
                                   dummyXThetaOut,
                                   *(yRadiusValues [rowClosest]),
                                   transformation);
  }
}

void Sim3Model
::calc_motion(const StereoCamera & cam,
              const ALIGNED<Vector3d>::vector & query_obs_vec,
              const ALIGNED<Vector3d>::vector & train_pt_vec,
              Transformation & sim3)
{
  assert(query_obs_vec.size()==num_points);
  assert(train_pt_vec.size()==num_points);

  // get centroids
  Vector3d p0a = cam.unmap_uvu(query_obs_vec[0]);
  Vector3d p0b = cam.unmap_uvu(query_obs_vec[1]);
  Vector3d p0c = cam.unmap_uvu(query_obs_vec[2]);
  Vector3d p1a = train_pt_vec[0];
  Vector3d p1b = train_pt_vec[1];
  Vector3d p1c = train_pt_vec[2];
  Vector3d c0, c1;
  Matrix3d R
      = AbsoluteOrientation::getOrientationAndCentriods(p0a,
                                                        p0b,
                                                        p0c,
                                                        p1a,
                                                        p1b,
                                                        p1c,
                                                        c0,
                                                        c1);
  sim3.set_rotation_matrix(R);
  sim3.scale() =
      sqrt(p0a.squaredNorm()
           + p0b.squaredNorm()
           + p0c.squaredNorm())
      /
      sqrt((R*p1a).squaredNorm()
           + (R*p1b).squaredNorm()
           + (R*p1c).squaredNorm());

  sim3.translation() = c0-sim3.scale()*R*c1;
}

 bool mapAndMultTest() {
   bool passed = true;
   for (size_t i=0; i<group_vec_.size(); ++i) {
     for (size_t j=0; j<group_vec_.size(); ++j) {
       Transformation mul_resmat = (group_vec_[i]*group_vec_[j]).matrix();
       Scalar fastmul_res_raw[LieGroup::num_parameters];
       Eigen::Map<LieGroup> fastmul_res(fastmul_res_raw);
       Eigen::Map<const LieGroup> group_j_constmap(group_vec_[j].data());
       fastmul_res = group_vec_[i];
       fastmul_res.fastMultiply(group_j_constmap);
       Transformation diff =  mul_resmat-fastmul_res.matrix();
       Scalar nrm = diff.norm();
       if (isnan(nrm) || nrm>SMALL_EPS) {
         cerr << "Map & Multiply" << endl;
         cerr  << "Test case: " << i  << "," << j << endl;
         cerr << diff <<endl;
         cerr << endl;
         passed = false;
       }
     }
   }
   return passed;
 }

bool DefaultLocalizer::fetch( const std::string& port, Transformation& out )
{
    requestState( ready );

    std::vector< CToolData* > tdv;

    CToolData td;
    td.bVisible = false;

    td.setSzPort( port );

    tdv.push_back( &td );

    pimpl->localizer.getToolData( tdv );

    if( td.isVisible() )
    {
        for( int i = 0; i < 3; ++i )
        for( int j = 0; j < 3; ++j )
        {
            out.getTransformationMatrix()( i, j ) = td.mR( i, j );
        }

        for( int i = 0; i < 3; ++i )
        {
            out.getTransformationMatrix()( i, 3 ) = td.vT( i );
        }

        out.a44() = 1;

        return true;
    }
    else
    {
        return false;
    }
}

void TurntableCamera<real>::Update()
{
	Transformation<real> transformation;
	Vector3<real> initRotationAxis = Vector3<real>( 0, 1, 0 ) ^ mThetaAxis;

	if ( initRotationAxis.SquaredLength() > 1e-5 )
		transformation.SetRotationAxis( initRotationAxis.Normalized(), asin( initRotationAxis.Length() ) );

	transformation.RotateAxis( mPhiAxis, mPhi );
	transformation.RotateAxis( mThetaAxis, mTheta );

	Transformation<real> transformationT;
	transformationT.RotateAxis( mPhiAxis, mPhi );
	transformationT.RotateAxis( mThetaAxis, mTheta );
	transformation.SetTranslation( transformationT.GetRotation() * ( mPhiAxis ^ mThetaAxis ).Normalized() * mDistance + mCenter );

	this->SetLocalTransformation( transformation );

	Camera<real>::Update();
}

void GraphicsScene::updateGraphicsLinesToMatchGraphicsPoints (const CurveStyles &curveStyles,
                                                              const Transformation &transformation)
{
  LOG4CPP_INFO_S ((*mainCat)) << "GraphicsScene::updateGraphicsLinesToMatchGraphicsPoints";

  if (transformation.transformIsDefined()) {

    // Ordinals must be updated to reflect reordering that may have resulted from dragging points
    m_graphicsLinesForCurves.updatePointOrdinalsAfterDrag (curveStyles,
                                                           transformation);

    // Recompute the lines one time for efficiency
    m_graphicsLinesForCurves.updateGraphicsLinesToMatchGraphicsPoints (curveStyles);
  }
}

void Checker::prepareForDisplay (const QPolygonF &polygon,
                                 int pointRadius,
                                 const DocumentModelAxesChecker &modelAxesChecker,
                                 const DocumentModelCoords &modelCoords,
                                 DocumentAxesPointsRequired documentAxesPointsRequired)
{
    LOG4CPP_INFO_S ((*mainCat)) << "Checker::prepareForDisplay";

    ENGAUGE_ASSERT ((polygon.count () == NUM_AXES_POINTS_3) ||
                    (polygon.count () == NUM_AXES_POINTS_4));

    // Convert pixel coordinates in QPointF to screen and graph coordinates in Point using
    // identity transformation, so this routine can reuse computations provided by Transformation
    QList<Point> points;
    QPolygonF::const_iterator itr;
    for (itr = polygon.begin (); itr != polygon.end (); itr++) {

        const QPointF &pF = *itr;

        Point p (DUMMY_CURVE_NAME,
                 pF,
                 pF,
                 false);
        points.push_back (p);
    }

    // Screen and graph coordinates are treated as the same, so identity transform is used
    Transformation transformIdentity;
    transformIdentity.identity();
    prepareForDisplay (points,
                       pointRadius,
                       modelAxesChecker,
                       modelCoords,
                       transformIdentity,
                       documentAxesPointsRequired);
}

  /// reorder points to upper-left-corner convention
  Point3dVector reorderULC(const Point3dVector& points)
  {
    unsigned N = points.size();
    if (N < 3){
      return Point3dVector();
    }

    // transformation to align face
    Transformation t = Transformation::alignFace(points);
    Point3dVector facePoints = t.inverse()*points;

    // find ulc index in face coordinates
    double maxY = std::numeric_limits<double>::min();
    double minX = std::numeric_limits<double>::max();
    unsigned ulcIndex = 0;
    for(unsigned i = 0; i < N; ++i){
      OS_ASSERT(std::abs(facePoints[i].z()) < 0.001);
      if ((maxY < facePoints[i].y()) || ((maxY < facePoints[i].y() + 0.00001) && (minX > facePoints[i].x()))){
        ulcIndex = i;
        maxY = facePoints[i].y();
        minX = facePoints[i].x();
      }
    }

    // no-op
    if (ulcIndex == 0){
      return points;
    }

    // create result
    Point3dVector result;
    std::copy (points.begin() + ulcIndex, points.end(), std::back_inserter(result));
    std::copy (points.begin(), points.begin() + ulcIndex, std::back_inserter(result));
    OS_ASSERT(result.size() == N);
    return result;
  }

	//---------------------------------------------------------------------------
	Impl(ColisionShapeType::Enum e, const Transformation& t, MeshObjectPtr colMesh)
		: mColShape(e)
		, mColMesh(colMesh)
		, mTransformation(t)
	{
		switch (e)
		{
		case ColisionShapeType::SPHERE:
		{
			mBV = BoundingVolume::Create(BoundingVolume::BV_SPHERE);
			auto scale = t.GetScale();
			if (scale.x != scale.z || scale.x != scale.y)
			{
				Logger::Log(FB_ERROR_LOG_ARG, "Collision Sphere should be uniform scaled!");
				assert(0);
			}
			mBV->SetRadius(1 * t.GetScale().x);
			mBV->SetCenter(Vec3::ZERO);
		}
		break;

		case ColisionShapeType::CUBE:
		{
			mBV = BoundingVolume::Create(BoundingVolume::BV_AABB);
			AABB aabb;
			aabb.SetMax(Vec3(1, 1, 1) * t.GetScale());
			aabb.SetMin(Vec3(-1, -1, -1) * t.GetScale());
			BVaabb* bvaabb = (BVaabb*)mBV.get();
			bvaabb->SetAABB(aabb);
		}
		break;

		default:
			break;
		}
	}

 /**
  * \brief Setter of quaternion using scaled rotation matrix
  *
  * \param sR a 3x3 scaled rotation matrix
  * \pre        the 3x3 matrix should be "scaled orthogonal"
  *             and have a positive determinant
  */
 inline
 void setScaledRotationMatrix
 (const Transformation & sR) {
   Transformation squared_sR = sR*sR.transpose();
   Scalar squared_scale
       = static_cast<Scalar>(1./3.)
       *(squared_sR(0,0)+squared_sR(1,1)+squared_sR(2,2));
   SOPHUS_ENSURE(squared_scale > static_cast<Scalar>(0),
                 "Scale factor should be positive");
   Scalar scale = std::sqrt(squared_scale);
   SOPHUS_ENSURE(scale > static_cast<Scalar>(0),
                 "Scale factor should be positive");
   quaternion() = sR/scale;
   quaternion().coeffs() *= scale;
 }

std::vector<FVector3> PointCloud::GetTransformedPositions(const Transformation& transformation) const
{
   std::vector<FVector3> transformedPositions;
      
   if (positions.size() > 0)
   {
      transformedPositions.reserve(positions.size());

      for (const FVector3& position : positions)
      {
         transformedPositions.push_back(transformation.MultVertex(position));
      }
   }

   return transformedPositions;
}

QPixmap GridRemoval::remove (const Transformation &transformation,
                             const DocumentModelGridRemoval &modelGridRemoval,
                             const QImage &imageBefore)
{
  LOG4CPP_INFO_S ((*mainCat)) << "GridRemoval::remove"
                              << " transformationIsDefined=" << (transformation.transformIsDefined() ? "true" : "false")
                              << " removeDefinedGridLines=" << (modelGridRemoval.removeDefinedGridLines() ? "true" : "false");

  QImage image = imageBefore;

  // Make sure grid line removal is wanted, and possible. Otherwise all processing is skipped
  if (modelGridRemoval.removeDefinedGridLines() &&
      transformation.transformIsDefined()) {

    double yGraphMin = modelGridRemoval.startY();
    double yGraphMax = modelGridRemoval.stopY();
    for (int i = 0; i < modelGridRemoval.countX(); i++) {
      double xGraph = modelGridRemoval.startX() + i * modelGridRemoval.stepX();

      // Convert line between graph coordinates (xGraph,yGraphMin) and (xGraph,yGraphMax) to screen coordinates
      QPointF posScreenMin, posScreenMax;
      transformation.transformRawGraphToScreen (QPointF (xGraph,
                                                         yGraphMin),
                                                posScreenMin);
      transformation.transformRawGraphToScreen (QPointF (xGraph,
                                                         yGraphMax),
                                                posScreenMax);

      removeLine (posScreenMin,
                  posScreenMax,
                  image);
    }

    double xGraphMin = modelGridRemoval.startX();
    double xGraphMax = modelGridRemoval.stopX();
    for (int j = 0; j < modelGridRemoval.countY(); j++) {
      double yGraph = modelGridRemoval.startY() + j * modelGridRemoval.stepY();

      // Convert line between graph coordinates (xGraphMin,yGraph) and (xGraphMax,yGraph) to screen coordinates
      QPointF posScreenMin, posScreenMax;
      transformation.transformRawGraphToScreen (QPointF (xGraphMin,
                                                         yGraph),
                                                posScreenMin);
      transformation.transformRawGraphToScreen (QPointF (xGraphMax,
                                                         yGraph),
                                                posScreenMax);

      removeLine (posScreenMin,
                  posScreenMax,
                  image);
    }
  }

  return QPixmap::fromImage (image);
}

double ExportFileFunctions::linearlyInterpolate (const Points &points,
                                                 double xThetaValue,
                                                 const Transformation &transformation) const
{
  LOG4CPP_INFO_S ((*mainCat)) << "ExportFileFunctions::linearlyInterpolate";

  double yRadius = 0;
  QPointF posGraphBefore; // Not set until ip=1
  bool foundIt = false;
  for (int ip = 0; ip < points.count(); ip++) {

    const Point &point = points.at (ip);
    QPointF posGraph;
    transformation.transformScreenToRawGraph (point.posScreen(),
                                                posGraph);

    if (xThetaValue <= posGraph.x()) {

      foundIt = true;
      if (ip == 0) {

        // Use first point
        yRadius = posGraph.y();

      } else {

        // Between posGraphBefore and posGraph. Note that if posGraph.x()=posGraphBefore.x() then
        // previous iteration of loop would have been used for interpolation, and then the loop was exited
        double s = (xThetaValue - posGraphBefore.x()) / (posGraph.x() - posGraphBefore.x());
        yRadius = (1.0 -s) * posGraphBefore.y() + s * posGraph.y();
      }

      break;
    }

    posGraphBefore = posGraph;
  }

  if (!foundIt) {

    // Use last point
    yRadius = posGraphBefore.y();

  }

  return yRadius;
}

void DrawPrimitiveQuad(Transformation &QuadTransformation, const EBlendMode &Mode, const ColorRGB &Color)
{
    Image::BindNull();
    WindowFrame.SetUniform(U_COLOR, Color.Red, Color.Green, Color.Blue, Color.Alpha);

    SetBlendingMode(Mode);

    Mat4 Mat = QuadTransformation.GetMatrix();
    WindowFrame.SetUniform(U_MVP, &(Mat[0][0]));

    // Assign position attrib. pointer
    SetPrimitiveQuadVBO();
    DoQuadDraw();
    FinalizeDraw();

    Image::ForceRebind();
}

void SceneNode::addTransformation(const Transformation& Trans)
{
	switch(Trans.getTransformationType()) {
	case Transformation::Translate:
		m_Transformations.push_back(new Translate((Translate&)Trans));
		break;
	case Transformation::Scale:
		m_Transformations.push_back(new Scale((Scale&)Trans));
		break;
	case Transformation::Rotate:
		m_Transformations.push_back(new Rotate((Rotate&)Trans));
		break;
	case Transformation::Matrix:
		m_Transformations.push_back(new Matrix((Matrix&)Trans));
		break;
	}
}

void Checker::finishActiveSegment (const DocumentModelCoords &modelCoords,
                                   const QPointF &posStartScreen,
                                   const QPointF &posEndScreen,
                                   double yFrom,
                                   double yTo,
                                   const Transformation &transformation,
                                   SideSegments &sideSegments) const
{
  LOG4CPP_INFO_S ((*mainCat)) << "Checker::finishActiveSegment"
                              << " posStartScreen=" << QPointFToString (posStartScreen).toLatin1().data()
                              << " posEndScreen=" << QPointFToString (posEndScreen).toLatin1().data()
                              << " yFrom=" << yFrom
                              << " yTo=" << yTo;

  QGraphicsItem *item;
  if ((modelCoords.coordsType() == COORDS_TYPE_POLAR) &&
      (yFrom == yTo)) {

    // Linear cartesian radius
    double radiusLinearCartesian = yFrom;
    if (modelCoords.coordScaleYRadius() == COORD_SCALE_LOG) {
      radiusLinearCartesian = transformation.logToLinearRadius(yFrom,
                                                               modelCoords.originRadius());
    } else {
      radiusLinearCartesian -= modelCoords.originRadius();
    }

    // Draw along an arc since this is a side of constant radius, and we have polar coordinates
    item = ellipseItem (transformation,
                        radiusLinearCartesian,
                        posStartScreen,
                        posEndScreen);

  } else {

    // Draw straight line
    item = lineItem (posStartScreen,
                     posEndScreen);
  }

  sideSegments.push_back (item);
  bindItemToScene (item);
}

void GridLineFactory::finishActiveGridLine (const QPointF &posStartScreen,
                                            const QPointF &posEndScreen,
                                            double yFrom,
                                            double yTo,
                                            const Transformation &transformation,
                                            GridLine &gridLine) const
{
  LOG4CPP_DEBUG_S ((*mainCat)) << "GridLineFactory::finishActiveGridLine"
                               << " posStartScreen=" << QPointFToString (posStartScreen).toLatin1().data()
                               << " posEndScreen=" << QPointFToString (posEndScreen).toLatin1().data()
                               << " yFrom=" << yFrom
                               << " yTo=" << yTo;

  QGraphicsItem *item;
  if ((m_modelCoords.coordsType() == COORDS_TYPE_POLAR) &&
      (yFrom == yTo)) {

    // Linear cartesian radius
    double radiusLinearCartesian = yFrom;
    if (m_modelCoords.coordScaleYRadius() == COORD_SCALE_LOG) {
      radiusLinearCartesian = transformation.logToLinearRadius(yFrom,
                                                               m_modelCoords.originRadius());
    } else {
      radiusLinearCartesian -= m_modelCoords.originRadius();
    }

    // Draw along an arc since this is a side of constant radius, and we have polar coordinates
    item = ellipseItem (transformation,
                        radiusLinearCartesian,
                        posStartScreen,
                        posEndScreen);

  } else {

    // Draw straight line
    item = lineItem (posStartScreen,
                     posEndScreen);
  }

  gridLine.add (item);
  bindItemToScene (item);
}

/* ------------------------------------------------------- */
Transformation Transformation::operator*(const Transformation& t)
{
	if (m_isIdentity)
	{
		return t;
	}

	if (t.m_isIdentity)
	{
		return *this;
	}

	Transformation result;
	if (m_isRSMatrix && t.m_isRSMatrix)
	{	
		if (m_isUniformScale)
		{
			result.setRotation(m_matrix * t.m_matrix);
			result.setTranslation(m_scale[0]*(m_matrix*t.m_translate) + m_translate);
			if (t.m_isUniformScale)
			{
				result.setUniformScale(m_scale[0] * t.m_scale[0]);
			}
			else
			{
				result.setScale(m_scale[0] * t.m_scale);
			}
			return result;
		}	
	}

	// In all remaining cases, the matrix cannot be written as R*S*X+T.
    Matrix3 a = ( m_isRSMatrix ?
        m_matrix.timesDiagonal(m_scale) :
        m_matrix );

    Matrix3 b = ( t.m_isRSMatrix ?
        t.m_matrix.timesDiagonal(t.m_scale) :
        t.m_matrix );

    result.setMatrix(a*b);
    result.setTranslation(a*t.m_translate + m_translate);
	return result;
}

void ExportOrdinalsSmooth::loadSplinePairsWithTransformation (const Points &points,
                                                              const Transformation &transformation,
                                                              vector<double> &t,
                                                              vector<SplinePair> &xy) const
{
  LOG4CPP_INFO_S ((*mainCat)) << "ExportOrdinalsSmooth::loadSplinePairsWithTransformation";

  Points::const_iterator itrP;
  for (itrP = points.begin(); itrP != points.end(); itrP++) {
    const Point &point = *itrP;
    QPointF posScreen = point.posScreen();
    QPointF posGraph;
    transformation.transformScreenToRawGraph (posScreen,
                                              posGraph);

    t.push_back (point.ordinal ());
    xy.push_back (SplinePair (posGraph.x(),
                              posGraph.y()));
  }
}