text: /*=========================================================================
   *
   *  Copyright Insight Software Consortium
   *
   *  Licensed under the Apache License, Version 2.0 (the "License");
   *  you may not use this file except in compliance with the License.
   *  You may obtain a copy of the License at
   *
   *         http://www.apache.org/licenses/LICENSE-2.0.txt
   *
   *  Unless required by applicable law or agreed to in writing, software
   *  distributed under the License is distributed on an "AS IS" BASIS,
   *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   *  See the License for the specific language governing permissions and
   *  limitations under the License.
   *
   *=========================================================================*/
  #ifndef itkScaleSkewVersor3DTransform_hxx
  #define itkScaleSkewVersor3DTransform_hxx
  
  #include "itkScaleSkewVersor3DTransform.h"
  #include "itkMath.h"
  
  namespace itk
  {
  // Constructor with default arguments
  template <typename TScalar>
  ScaleSkewVersor3DTransform<TScalar>
  ::ScaleSkewVersor3DTransform() :
    Superclass(ParametersDimension)
  {
    m_Scale.Fill(NumericTraits<TScalar>::One);
    m_Skew.Fill(NumericTraits<TScalar>::Zero);
  }
  
  // Constructor with arguments
  template <typename TScalar>
  ScaleSkewVersor3DTransform<TScalar>::ScaleSkewVersor3DTransform(unsigned int parametersDimension) :
    Superclass(parametersDimension)
  {
    m_Scale.Fill(1.0);
    m_Skew.Fill(0.0);
  }
  
  // Constructor with arguments
  template <typename TScalar>
  ScaleSkewVersor3DTransform<TScalar>::ScaleSkewVersor3DTransform(const MatrixType & matrix,
                                                                      const OutputVectorType & offset) :
    Superclass(matrix, offset)
  {
    this->ComputeMatrixParameters();
  }
  
  // Directly set the matrix
  template <typename TScalar>
  void
  ScaleSkewVersor3DTransform<TScalar>
  ::SetMatrix(const MatrixType & matrix)
  {
    // Any matrix should work - bypass orthogonality testing
    typedef MatrixOffsetTransformBase<TScalar, 3> Baseclass;
    this->Baseclass::SetMatrix(matrix);
  }
  
  template <typename TScalar>
  void
  ScaleSkewVersor3DTransform<TScalar>
  ::SetMatrix(const MatrixType & matrix, double itkNotUsed( tolerance ))
  {
    // Any matrix should work - bypass orthogonality testing
    typedef MatrixOffsetTransformBase<TScalar, 3> Baseclass;
    this->Baseclass::SetMatrix(matrix);
  }
  
  // Set Parameters
  template <typename TScalar>
  void
  ScaleSkewVersor3DTransform<TScalar>
  ::SetParameters(const ParametersType & parameters)
  {
    itkDebugMacro(<< "Setting parameters " << parameters);
  
    // Save parameters. Needed for proper operation of TransformUpdateParameters.
    if( &parameters != &(this->m_Parameters) )
      {
      this->m_Parameters = parameters;
      }
  
    // Transfer the versor part
  
    AxisType axis;
  
    double norm = parameters[0] * parameters[0];
    axis[0] = parameters[0];
    norm += parameters[1] * parameters[1];
    axis[1] = parameters[1];
    norm += parameters[2] * parameters[2];
    axis[2] = parameters[2];
    if( norm > 0 )
      {
      norm = std::sqrt(norm);
      }
  
    double epsilon = 1e-10;
    if( norm >= 1.0 - epsilon )
      {
      axis = axis / ( norm + epsilon * norm );
      }
    VersorType newVersor;
    newVersor.Set(axis);
    this->SetVarVersor(newVersor);
  
    itkDebugMacro(<< "Versor is now " << newVersor);
  
    // Matrix must be defined before translation so that offset can be computed
    // from translation
    m_Scale[0] = parameters[6];
    m_Scale[1] = parameters[7];
    m_Scale[2] = parameters[8];
  
    m_Skew[0] = parameters[9];
    m_Skew[1] = parameters[10];
    m_Skew[2] = parameters[11];
    m_Skew[3] = parameters[12];
    m_Skew[4] = parameters[13];
    m_Skew[5] = parameters[14];
  
    // Transfer the translation part
    TranslationType newTranslation;
    newTranslation[0] = parameters[3];
    newTranslation[1] = parameters[4];
    newTranslation[2] = parameters[5];
  
    this->SetVarTranslation(newTranslation);
    this->ComputeMatrix();
    this->ComputeOffset();
  
    // Modified is always called since we just have a pointer to the
    // parameters and cannot know if the parameters have changed.
    this->Modified();
  
    itkDebugMacro(<< "After setting parameters ");
  }
  
  //
  // Get Parameters
  //
  // Parameters are ordered as:
  //
  // p[0:2] = right part of the versor (axis times std::sin(t/2))
  // p[3:5] = translation components
  // p[6:8] = Scale
  // p[9:14] = Skew {xy, xz, yx, yz, zx, zy}
  //
  
  template <typename TScalar>
  const typename ScaleSkewVersor3DTransform<TScalar>::ParametersType
  & ScaleSkewVersor3DTransform<TScalar>
  ::GetParameters(void) const
    {
    itkDebugMacro(<< "Getting parameters ");
  
    this->m_Parameters[0] = this->GetVersor().GetX();
    this->m_Parameters[1] = this->GetVersor().GetY();
    this->m_Parameters[2] = this->GetVersor().GetZ();
  
    this->m_Parameters[3] = this->GetTranslation()[0];
    this->m_Parameters[4] = this->GetTranslation()[1];
    this->m_Parameters[5] = this->GetTranslation()[2];
  
    this->m_Parameters[6] = this->GetScale()[0];
    this->m_Parameters[7] = this->GetScale()[1];
    this->m_Parameters[8] = this->GetScale()[2];
  
    this->m_Parameters[9] = this->GetSkew()[0];
    this->m_Parameters[10] = this->GetSkew()[1];
    this->m_Parameters[11] = this->GetSkew()[2];
    this->m_Parameters[12] = this->GetSkew()[3];
    this->m_Parameters[13] = this->GetSkew()[4];
    this->m_Parameters[14] = this->GetSkew()[5];
  
    itkDebugMacro(<< "After getting parameters " << this->m_Parameters);
  
    return this->m_Parameters;
    }
  
  template <typename TScalar>
  void
  ScaleSkewVersor3DTransform<TScalar>
  ::SetIdentity()
  {
    m_Scale.Fill(NumericTraits<ScaleVectorValueType>::One);
    m_Skew.Fill(NumericTraits<SkewVectorValueType>::Zero);
    Superclass::SetIdentity();
  }
  
  template <typename TScalar>
  void
  ScaleSkewVersor3DTransform<TScalar>
  ::SetScale(const ScaleVectorType & scale)
  {
    m_Scale = scale;
    this->ComputeMatrix();
  }
  
  template <typename TScalar>
  void
  ScaleSkewVersor3DTransform<TScalar>
  ::SetSkew(const SkewVectorType & skew)
  {
    m_Skew = skew;
    this->ComputeMatrix();
  }
  
  // Compute the matrix
  template <typename TScalar>
  void
  ScaleSkewVersor3DTransform<TScalar>
  ::ComputeMatrix(void)
  {
    this->Superclass::ComputeMatrix();
  
    MatrixType newMatrix = this->GetMatrix();
  
    newMatrix[0][0] += m_Scale[0] - 1.0;
    newMatrix[1][1] += m_Scale[1] - 1.0;
    newMatrix[2][2] += m_Scale[2] - 1.0;
    newMatrix[0][1] += m_Skew[0];
    newMatrix[0][2] += m_Skew[1];
    newMatrix[1][0] += m_Skew[2];
    newMatrix[1][2] += m_Skew[3];
    newMatrix[2][0] += m_Skew[4];
    newMatrix[2][1] += m_Skew[5];
  
    this->SetVarMatrix(newMatrix);
  }
  
  template <typename TScalar>
  void
  ScaleSkewVersor3DTransform<TScalar>
  ::ComputeMatrixParameters(void)
  {
    itkExceptionMacro(<< "Setting the matrix of a ScaleSkewVersor3D transform is not supported at this time.");
  }
  
  // Print self
  template <typename TScalar>
  void
  ScaleSkewVersor3DTransform<TScalar>::PrintSelf(std::ostream & os, Indent indent) const
  {
    Superclass::PrintSelf(os, indent);
  
    os << indent << "Scale:       " << m_Scale        << std::endl;
    os << indent << "Skew:        " << m_Skew         << std::endl;
  }
  
  template <typename TScalar>
  void
  ScaleSkewVersor3DTransform<TScalar>
  ::ComputeJacobianWithRespectToParameters(const InputPointType & p, JacobianType & jacobian) const
  {
    typedef typename VersorType::ValueType ValueType;
  
    // compute derivatives with respect to rotation
    const ValueType vx = this->GetVersor().GetX();
    const ValueType vy = this->GetVersor().GetY();
    const ValueType vz = this->GetVersor().GetZ();
    const ValueType vw = this->GetVersor().GetW();
  
    jacobian.SetSize( 3, this->GetNumberOfLocalParameters() );
    jacobian.Fill(0.0);
  
    const double px = p[0] - this->GetCenter()[0];
    const double py = p[1] - this->GetCenter()[1];
    const double pz = p[2] - this->GetCenter()[2];
  
    const double vxx = vx * vx;
    const double vyy = vy * vy;
    const double vzz = vz * vz;
    const double vww = vw * vw;
  
    const double vxy = vx * vy;
    const double vxz = vx * vz;
    const double vxw = vx * vw;
  
    const double vyz = vy * vz;
    const double vyw = vy * vw;
  
    const double vzw = vz * vw;
  
    // compute Jacobian with respect to quaternion parameters
    jacobian[0][0] = 2.0 * ( ( vyw + vxz ) * py + ( vzw - vxy ) * pz )
      / vw;
    jacobian[1][0] = 2.0 * ( ( vyw - vxz ) * px   - 2 * vxw   * py + ( vxx - vww ) * pz )
      / vw;
    jacobian[2][0] = 2.0 * ( ( vzw + vxy ) * px + ( vww - vxx ) * py   - 2 * vxw   * pz )
      / vw;
  
    jacobian[0][1] = 2.0 * ( -2 * vyw  * px + ( vxw + vyz ) * py + ( vww - vyy ) * pz )
      / vw;
    jacobian[1][1] = 2.0 * ( ( vxw - vyz ) * px                + ( vzw + vxy ) * pz )
      / vw;
    jacobian[2][1] = 2.0 * ( ( vyy - vww ) * px + ( vzw - vxy ) * py   - 2 * vyw   * pz )
      / vw;
  
    jacobian[0][2] = 2.0 * ( -2 * vzw  * px + ( vzz - vww ) * py + ( vxw - vyz ) * pz )
      / vw;
    jacobian[1][2] = 2.0 * ( ( vww - vzz ) * px   - 2 * vzw   * py + ( vyw + vxz ) * pz )
      / vw;
    jacobian[2][2] = 2.0 * ( ( vxw + vyz ) * px + ( vyw - vxz ) * py )
      / vw;
  
    jacobian[0][3] = 1.0;
    jacobian[1][4] = 1.0;
    jacobian[2][5] = 1.0;
  
    jacobian[0][6] = px;
    jacobian[1][7] = py;
    jacobian[2][8] = pz;
  
    jacobian[0][9]  = py;
    jacobian[0][10] = pz;
    jacobian[1][11] = px;
    jacobian[1][12] = pz;
    jacobian[2][13] = px;
    jacobian[2][14] = py;
  }
  
  } // namespace
  
  #endif
  