---

distance_constraint.cpp

//$Header: /home/ben/Mapper/c++/RCS/distance_constraint.cpp,v 6.4 2002/07/07 18:23:13 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 <cassert>
#include <cmath>

#include "distance_constraint.h"
#include "vector2.h"



distance_constraint::distance_constraint(
   const double u,
   const double d
   )
   :
   constraint( u ),
   wanted_distance_( d )
{
   {//preconditions:
      assert( 0.0 < u );
      assert( 0.0 <= d );
   };
   this->points_.reserve( 2 );
   static const vector< vector2* > zero( 2U, 0 );
   this->points_ = zero;
   {//postconditions:
      assert( this->uncertainty() == u );
      assert( this->wanted_distance() == d );
      assert( this->points().size() == 2 );
      assert( this->points()[0] == 0 );
      assert( this->points()[1] == 0 );
   };
}



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



void distance_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 p[i],
   //and the rates of change of that energy dedp
   //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 );
   };
   const vector2 v( p[1] - p[0] );
   const double r2 = v * v;
   const double r = sqrt( r2 );
   const double re = (r - this->wanted_distance())/this->uncertainty();
   e = re*re;
   {//ensure the output vector haathe correct dimension
      static const vector< vector2 > empty2( 2 );
      dedp = empty2;
   };
   if( 0.0 < r2 ){
      const vector2 grad( v * (2.0*re/(this->uncertainty() * r2)) );
      dedp[1] = grad;
      dedp[0] = -grad;
   }else{ //special case: coincident points (local maximum)
      dedp[0] = vector2( 0, 0 );
      dedp[1] = vector2( 0, 0 );
   };
   {//postconditions:
      assert( dedp.size() == 2 );
   };
}



void distance_constraint::wanted_distance(
   const double d
   )
{
   {//preconditions:
      assert( 0.0 <= d );
   };
   this->wanted_distance_ = d;
   {//postconditions:
      assert( this->wanted_distance() == d );
   };
}

---