constraint.cpp

/****************************************************************************

 Copyright (C) 2002-2008 Gilles Debunne. All rights reserved.

 This file is part of the QGLViewer library version 2.3.10.

 http://www.libqglviewer.com - contact@libqglviewer.com

 This file may be used under the terms of the GNU General Public License 
 versions 2.0 or 3.0 as published by the Free Software Foundation and
 appearing in the LICENSE file included in the packaging of this file.
 In addition, as a special exception, Gilles Debunne gives you certain 
 additional rights, described in the file GPL_EXCEPTION in this package.

 libQGLViewer uses dual licensing. Commercial/proprietary software must
 purchase a libQGLViewer Commercial License.

 This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.

*****************************************************************************/

#include "constraint.h"
#include "frame.h"
#include "camera.h"

using namespace qglviewer;
using namespace std;

//                                  Constraint                                //

AxisPlaneConstraint::AxisPlaneConstraint()
  : translationConstraintType_(FREE), rotationConstraintType_(FREE)
{
  // Do not use set since setRotationConstraintType needs a read.
}

void AxisPlaneConstraint::setTranslationConstraint(Type type, const Vec& direction)
{
  setTranslationConstraintType(type);
  setTranslationConstraintDirection(direction);
}

void AxisPlaneConstraint::setTranslationConstraintDirection(const Vec& direction)
{
  if ((translationConstraintType()!=AxisPlaneConstraint::FREE) && (translationConstraintType()!=AxisPlaneConstraint::FORBIDDEN))
    {
      const float norm = direction.norm();
      if (norm < 1E-8)
    {
      qWarning("AxisPlaneConstraint::setTranslationConstraintDir: null vector for translation constraint");
      translationConstraintType_ = AxisPlaneConstraint::FREE;
    }
      else
    translationConstraintDir_ = direction/norm;
    }
}

void AxisPlaneConstraint::setRotationConstraint(Type type, const Vec& direction)
{
  setRotationConstraintType(type);
  setRotationConstraintDirection(direction);
}

void AxisPlaneConstraint::setRotationConstraintDirection(const Vec& direction)
{
  if ((rotationConstraintType()!=AxisPlaneConstraint::FREE) && (rotationConstraintType()!=AxisPlaneConstraint::FORBIDDEN))
    {
      float norm = direction.norm();
      if (norm < 1E-8)
    {
      qWarning("AxisPlaneConstraint::setRotationConstraintDir: null vector for rotation constraint");
      rotationConstraintType_ = AxisPlaneConstraint::FREE;
    }
      else
    rotationConstraintDir_ = direction/norm;
    }
}

void AxisPlaneConstraint::setRotationConstraintType(Type type)
{
  if (rotationConstraintType() == AxisPlaneConstraint::PLANE)
    {
      qWarning("AxisPlaneConstraint::setRotationConstraintType: the PLANE type cannot be used for a rotation constraints");
      return;
    }

  rotationConstraintType_ = type;
}


//                               LocalConstraint                              //

void LocalConstraint::constrainTranslation(Vec& translation, Frame* const frame)
{
  Vec proj;
  switch (translationConstraintType())
    {
    case AxisPlaneConstraint::FREE:
      break;
    case AxisPlaneConstraint::PLANE:
      proj = frame->rotation().rotate(translationConstraintDirection());
      translation.projectOnPlane(proj);
      break;
    case AxisPlaneConstraint::AXIS:
      proj = frame->rotation().rotate(translationConstraintDirection());
      translation.projectOnAxis(proj);
      break;
    case AxisPlaneConstraint::FORBIDDEN:
      translation = Vec(0.0, 0.0, 0.0);
      break;
    }
}

void LocalConstraint::constrainRotation(Quaternion& rotation, Frame* const)
{
  switch (rotationConstraintType())
    {
    case AxisPlaneConstraint::FREE:
      break;
    case AxisPlaneConstraint::PLANE:
      break;
    case AxisPlaneConstraint::AXIS:
      {
    Vec axis = rotationConstraintDirection();
    Vec quat = Vec(rotation[0], rotation[1], rotation[2]);
    quat.projectOnAxis(axis);
    rotation = Quaternion(quat, 2.0*acos(rotation[3]));
      }
      break;
    case AxisPlaneConstraint::FORBIDDEN:
      rotation = Quaternion(); // identity
      break;
    }
}

//                               WorldConstraint                              //

void WorldConstraint::constrainTranslation(Vec& translation, Frame* const frame)
{
  Vec proj;
  switch (translationConstraintType())
    {
    case AxisPlaneConstraint::FREE:
      break;
    case AxisPlaneConstraint::PLANE:
      if (frame->referenceFrame())
    {
      proj = frame->referenceFrame()->transformOf(translationConstraintDirection());
      translation.projectOnPlane(proj);
    }
      else
    translation.projectOnPlane(translationConstraintDirection());
      break;
    case AxisPlaneConstraint::AXIS:
      if (frame->referenceFrame())
    {
      proj = frame->referenceFrame()->transformOf(translationConstraintDirection());
      translation.projectOnAxis(proj);
    }
      else
    translation.projectOnAxis(translationConstraintDirection());
      break;
    case AxisPlaneConstraint::FORBIDDEN:
      translation = Vec(0.0, 0.0, 0.0);
      break;
    }
}

void WorldConstraint::constrainRotation(Quaternion& rotation, Frame* const frame)
{
  switch (rotationConstraintType())
    {
    case AxisPlaneConstraint::FREE:
      break;
    case AxisPlaneConstraint::PLANE:
      break;
    case AxisPlaneConstraint::AXIS:
      {
    Vec quat(rotation[0], rotation[1], rotation[2]);
    Vec axis = frame->transformOf(rotationConstraintDirection());
    quat.projectOnAxis(axis);
    rotation = Quaternion(quat, 2.0*acos(rotation[3]));
    break;
      }
    case AxisPlaneConstraint::FORBIDDEN:
      rotation = Quaternion(); // identity
      break;
    }
}

//                               CameraConstraint                              //

CameraConstraint::CameraConstraint(const Camera* const camera)
  : AxisPlaneConstraint(), camera_(camera)
{}

void CameraConstraint::constrainTranslation(Vec& translation, Frame* const frame)
{
  Vec proj;
  switch (translationConstraintType())
    {
    case AxisPlaneConstraint::FREE:
      break;
    case AxisPlaneConstraint::PLANE:
      proj = camera()->frame()->inverseTransformOf(translationConstraintDirection());
      if (frame->referenceFrame())
    proj = frame->referenceFrame()->transformOf(proj);
      translation.projectOnPlane(proj);
      break;
    case AxisPlaneConstraint::AXIS:
      proj = camera()->frame()->inverseTransformOf(translationConstraintDirection());
      if (frame->referenceFrame())
    proj = frame->referenceFrame()->transformOf(proj);
      translation.projectOnAxis(proj);
      break;
    case AxisPlaneConstraint::FORBIDDEN:
      translation = Vec(0.0, 0.0, 0.0);
      break;
    }
}

void CameraConstraint::constrainRotation(Quaternion& rotation, Frame* const frame)
{
  switch (rotationConstraintType())
    {
    case AxisPlaneConstraint::FREE:
      break;
    case AxisPlaneConstraint::PLANE:
      break;
    case AxisPlaneConstraint::AXIS:
      {
    Vec axis = frame->transformOf(camera()->frame()->inverseTransformOf(rotationConstraintDirection()));
    Vec quat = Vec(rotation[0], rotation[1], rotation[2]);
    quat.projectOnAxis(axis);
    rotation = Quaternion(quat, 2.0*acos(rotation[3]));
      }
      break;
    case AxisPlaneConstraint::FORBIDDEN:
      rotation = Quaternion(); // identity
      break;
    }
}