---

angle_constraint.cpp

//$Header: /home/ben/Mapper/c++/RCS/angle_constraint.cpp,v 6.4 2002/07/02 13:14:38 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 "angle_constraint.h"



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



static const double pi = M_PI;



angle_constraint::angle_constraint(
   const double u,
   const double a
   )
   :
   constraint( u ),
   wanted_angle_( a )
{
   {//preconditions:
      assert( 0.0 < u );
   };
   this->points_.reserve( 3 );
   const vector< vector2* > zero( 3U, 0 );
   this->points_ = zero;
   {//postconditions:
      assert( this->uncertainty() == u );
      assert( this->wanted_angle() == a );
      assert( this->points().size() == 3 );
   };
}



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


namespace {

   static double normalised_angle(
      double a
      )
      {
         while( pi < a ) a -= 2.0*pi;
         while( a < -pi ) a += 2.0*pi;
         assert( -pi <= a && a <= pi );
         return a;
     }



   static void gradients(
      const double r2,
      const vector2 &v,
      const double deda,
      vector2 &dedp //output
      )
      {
         vector2 dadp;
         if( 0.0 < r2 )
            dadp = vector2( -v.y/r2, v.x/r2 );
         else
            //special case
            dadp = vector2( 0, 0 );
         ;
         dedp = deda * dadp;
      };

}



void angle_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() == 3 );
      //assert( 0 < tol && tol < 1 );
   };
   const vector2 v1( p[1] - p[0] );
   const vector2 v2( p[2] - p[0] );
   const double r12 = v1 * v1;
   const double r22 = v2 * v2;
   const double a1 = atan2( v1.y, v1.x );
   const double a2 = atan2( v2.y, v2.x );
   const double a = a2 - a1;
   double ae = normalised_angle( a - this->wanted_angle() );
   //calculate energy
   const double aeu = ae/this->uncertainty();
   e =  aeu*aeu;
   {//calculate gradients
      const double w = 2.0*aeu/this->uncertainty();
      const double deda1 = -w;
      const double deda2 = w;
      {//ensure the output vector has the correct dimension
         static const vector< vector2 > empty3( 3 );
         dedp = empty3;
      };
      gradients( r12, v1, deda1, dedp[1] );
      gradients( r22, v2, deda2, dedp[2] );
      dedp[0] = -(dedp[1] + dedp[2]);
   };
   {//postconditions:
      assert( dedp.size() == 3 );
   };
}

---