---

bearing_constraint.cpp

//$Header: /home/ben/Mapper/c++/RCS/bearing_constraint.cpp,v 6.2 2002/06/14 22:28:47 ben Exp $
// Copyright Benedict Adamson 2002.
// This file is part of Mapper.

// Mapper is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.

// Mapper is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.

// You should have received a copy of the GNU General Public License
// along with Mapper; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#include "bearing_constraint.h"



#include <cmath>
#include "vector2.h"



static const double pi = M_PI;



bearing_constraint::bearing_constraint(
   const double u,
   const double b
   )
   :
   constraint( u ),
   wanted_bearing_( b )
{
   this->points_.reserve( 2 );
   const vector< vector2* > zero( 2U, 0 );
   this->points_ = zero;
   {//postconditions:
      assert( this->uncertainty() == u );
      assert( this->wanted_bearing() == b );
      assert( this->points().size() == 2 );
   };
}



bearing_constraint::~bearing_constraint(void)
{
   if( this->points_[0] ) unassoc( *this );
}



void bearing_constraint::energy(
   const double /*tol*/,
   const vector< vector2 > &p,
   double &e, //output
   vector< vector2 > &dedp //output
   ) const
{
   //tol is a dimensionless calculation tolerance.
   //Calculate a dimensionless measure of the potential energy (e)
   //of the constraint,
   //if this->(*point)[i] had position (x[i], y[i]),
   //and the rates of change of that energy (dedx, dedy)
   //with respect to the point positions.
   //Larger values indicate the constraint is less well satisfied.
   //Conventionally, 0 is defined as the minimum possible energy,
   //And energy <= 1 indicates that the constraint is satisifed
   //to within the uncertainty of the constraint.
   {//precondition:
      assert( p.size() == 2 );
      //assert( 0 < tol && tol < 1 );
   };
   {//ensure returned gradient vector has the correct dimension
     static const vector< vector2 > empty( 2 );
     dedp = empty;
   };
   const vector2 v( p[1] - p[0] );
   const double r2 = v*v;
   if( 0.0 < r2 ){
      //calculate angles
      const double a = atan2( v.x, v.y );
      double da = a - this->wanted_bearing();
      while( pi < da ) da -= 2.0*pi;
      while( da < -pi ) da += 2.0*pi;
      assert( -pi <= da && da <= pi );
      //calculate energy
      const double ae = da / this->uncertainty();
      e =  ae*ae;
      {//calculate gradients
         const vector2 grad(
            vector2( -v.y, v.x ) * 2.0*ae/
            (r2 * this->uncertainty())
            );
         dedp[0] = grad;
         dedp[1] = -grad;
      };
   }else{
      e = 0.0;
      dedp[0] = vector2( 0, 0 );
      dedp[1] = vector2( 0, 0 );
   };
   {//postconditions:
      assert( dedp.size() == 2 );
   };
}



void bearing_constraint::wanted_bearing(
   const double d
   )
{
   {//preconditions:
      assert( 0.0 <= d );
   };
   this->wanted_bearing_ = d;
   {//postconditions:
      assert( this->wanted_bearing() == d );
   };
}

---