CoordinateSystem.cc

Go to the documentation of this file.

#include <utl/CoordinateSystem.h>
#include <utl/TransformationMatrix.h>
#include <vector>
#include <algorithm>

using namespace utl;


CoordinateTransformer::CoordinateTransformer(const TransformationMatrix& theTransformation,
                                             const CoordinateSystemPtr& theReferenceCS) :
  fTransformation(theReferenceCS->GetTransformationForNewCS(theTransformation)),
  fReferenceSystem(theReferenceCS->GetReferenceForNewCS())
{
  CoordinateSystemValid(theReferenceCS);
}


CoordinateTransformer::~CoordinateTransformer()
{
  delete fLastTransformation;
  delete fLastInverseTransformation;
}


CoordinateSystemPtr
CoordinateTransformer::GetThis()
  const
{
  return shared_from_this();
}


CoordinateSystemPtr CoordinateTransformer::RootCoordinateSystem = CoordinateSystemPtr();


CoordinateSystemPtr
CoordinateTransformer::GetRootCoordinateSystem()
{
  if (!RootCoordinateSystem)
    RootCoordinateSystem = CoordinateSystemPtr(new BaseCSPolicy());
  return RootCoordinateSystem;
}


CoordinateSystemPtr
DerivedCSPolicy::GetReferenceForNewCS()
  const
{
  return GetReferenceSystem();
}


CoordinateSystemPtr
BaseCSPolicy::GetReferenceForNewCS()
  const
{
  return GetThis();
}


TransformationMatrix
DerivedCSPolicy::GetTransformationForNewCS(const TransformationMatrix& theTransformation)
  const
{
  return theTransformation * GetTransformation();
}


TransformationMatrix
BaseCSPolicy::GetTransformationForNewCS(const TransformationMatrix& theTransformation)
  const
{
  return theTransformation;
}


CoordinateSystemPtr
DerivedCSPolicy::FromTransformation(const TransformationMatrix& theTrafo,
                                    const CoordinateSystemPtr& theCS)
{
  return CoordinateSystemPtr(new DerivedCSPolicy(theTrafo.Inverse(), theCS));
}


CoordinateSystemPtr
BaseCSPolicy::FromTransformation(const TransformationMatrix& theTrafo,
                                 const CoordinateSystemPtr& theCS)
{
  return CoordinateSystemPtr(new BaseCSPolicy(theTrafo.Inverse(), theCS));
}


CoordinateSystemPtr
CoordinateTransformer::FindCommonBase(const CoordinateSystemPtr& other)
  const
{
  const CoordinateSystemPtr root = GetRootCoordinateSystem();

  std::vector<CoordinateSystemPtr> myBases;
  for (auto cs = GetReferenceSystem(); cs != root; cs = cs->GetReferenceSystem())
    myBases.push_back(cs);

  for (auto cs = other->GetReferenceSystem(); cs != root; cs = cs->GetReferenceSystem()) {
    const auto end = myBases.cend();
    if (std::find(myBases.cbegin(), end, cs) != end)
      return cs;
  }
  // not found, so the root CS must be the only common ancestor
  return root;
}


TransformationMatrix
CoordinateTransformer::CollectTransformationFrom(const CoordinateSystemPtr& target)
  const
{
  TransformationMatrix result(GetTransformation());

  for (auto cs = GetReferenceSystem(); cs != target; cs = cs->GetReferenceSystem())
    result *= cs->GetTransformation();

  return result;
}


const TransformationMatrix&
CoordinateTransformer::GetTransformationTo(const CoordinateSystemPtr& target)
  const
{
  if (target != fLastTarget.lock() || !fLastTransformation) {
    delete fLastTransformation;
    fLastTarget = target;

    const CoordinateSystemPtr commonBase = FindCommonBase(target);

    fLastTransformation =
      new TransformationMatrix(
        target->CollectTransformationFrom(commonBase) *
        CollectTransformationFrom(commonBase).Inverse()
      );
  }

  return *fLastTransformation;
}


const TransformationMatrix&
CoordinateTransformer::GetInverseTransformationTo(const CoordinateSystemPtr& target)
  const
{
  if (target != fLastTarget.lock() || !fLastInverseTransformation) {
    delete fLastInverseTransformation;
    fLastInverseTransformation =
      new TransformationMatrix(GetTransformationTo(target).Inverse());
  }

  return *fLastInverseTransformation;
}