robFunctionSO3.h

Go to the documentation of this file.

/*
  Author(s): Simon Leonard
  Created on: Nov 11 2009

  (C) Copyright 2009-2014 Johns Hopkins University (JHU), All Rights
  Reserved.

--- begin cisst license - do not edit ---

This software is provided "as is" under an open source license, with
no warranty.  The complete license can be found in license.txt and
http://www.cisst.org/cisst/license.txt.

--- end cisst license ---
*/

#ifndef _robFunctionSO3_h
#define _robFunctionSO3_h

#include <cisstVector/vctFixedSizeVector.h>
#include <cisstVector/vctMatrixRotation3.h>
#include <cisstVector/vctQuaternionRotation3.h>

#include <cisstRobot/robFunction.h>
#include <cisstRobot/robExport.h>

class CISST_EXPORT robFunctionSO3 : public robFunction {

 protected:

  vctQuaternionRotation3<double> qw1;  // initial value
  vctFixedSizeVector<double,3>    w1;  // initial 1st derivative
  vctFixedSizeVector<double,3>   w1d;  // initial 1st derivative

  vctQuaternionRotation3<double> qw2;  // final value
  vctFixedSizeVector<double,3>    w2;  // final 1st derivative
  vctFixedSizeVector<double,3>   w2d;  // final 1st derivative

 public:

  robFunctionSO3( void );

  robFunctionSO3( double t1,
                  const vctQuaternionRotation3<double>& qw1,
                  const vctFixedSizeVector<double,3>&    w1,
                  const vctFixedSizeVector<double,3>&   w1d,
                  double t2,
                  const vctQuaternionRotation3<double>& qw2,
                  const vctFixedSizeVector<double,3>&    w2,
                  const vctFixedSizeVector<double,3>&   w2d );

  robFunctionSO3( double t1,
                  const vctMatrixRotation3<double>&   Rw1,
                  const vctFixedSizeVector<double,3>&  w1,
                  const vctFixedSizeVector<double,3>& w1d,
                  double t2,
                  const vctMatrixRotation3<double>&   Rw2,
                  const vctFixedSizeVector<double,3>&  w2,
                  const vctFixedSizeVector<double,3>& w2d );

  virtual ~robFunctionSO3() {}

  virtual void Evaluate( double,
                         vctQuaternionRotation3<double>&,
                         vctFixedSizeVector<double,3>&,
                         vctFixedSizeVector<double,3>& ){}

  virtual void Evaluate( double,
                         vctMatrixRotation3<double>,
                         vctFixedSizeVector<double,3>&,
                         vctFixedSizeVector<double,3>& ){}

  virtual void Blend( robFunction* function, double wmax, double wdmax ) = 0;

  void InitialState( vctQuaternionRotation3<double>& q,
                     vctFixedSizeVector<double,3>& w,
                     vctFixedSizeVector<double,3>& wd );

  void FinalState( vctQuaternionRotation3<double>& q,
                   vctFixedSizeVector<double,3>& w,
                   vctFixedSizeVector<double,3>& wd );

};

#endif // _robFunctionSO3_h