/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include "../../headers/random.h"

#include "Sample.h"

void Sample::add_der_to_sample(const Sample *der)
{
  loc   += der->loc;
  angle += der->angle;
}

void Sample::copy_der(const Sample *other_der)
{
  loc   = other_der->loc;
  angle = other_der->angle;
}

void Sample::set_der_zero()
{
  loc.set(0.0,0.0);
  angle = 0.0;
}

void Sample::add_der(const Sample *other_der)
{
  loc   += other_der->loc;
  angle += other_der->angle;
}

void Sample::scale_der(double val)
{
  loc   *= val;
  angle *= val;
}

void Sample::random_momentum(Random &random)
{
  loc.set(GlobalGaussianSampler.gen_sample(),GlobalGaussianSampler.gen_sample());
  angle = GlobalGaussianSampler.gen_sample();
}

double Sample::sqlength()
{
  double sqlen = 0.0;

  sqlen += loc.sqlength();
  sqlen += angle*angle;

  return sqlen;
}