intern/iksolver/intern/IK_QTask.h

   1 /*
   2  * ***** BEGIN GPL LICENSE BLOCK *****
   3  *
   4  * This program is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU General Public License
   6  * as published by the Free Software Foundation; either version 2
   7  * of the License, or (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the Free Software Foundation,
  16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
  17  *
  18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
  19  * All rights reserved.
  20  *
  21  * The Original Code is: all of this file.
  22  *
  23  * Original author: Laurence
  24  * Contributor(s): Brecht
  25  *
  26  * ***** END GPL LICENSE BLOCK *****
  27  */
  28
  29 /** \file iksolver/intern/IK_QTask.h
  30  *  \ingroup iksolver
  31  */
  32
  33
  34 #ifndef __IK_QTASK_H__
  35 #define __IK_QTASK_H__
  36
  37 #include "MT_Vector3.h"
  38 #include "MT_Transform.h"
  39 #include "MT_Matrix4x4.h"
  40 #include "IK_QJacobian.h"
  41 #include "IK_QSegment.h"
  42
  43 class IK_QTask
  44 {
  45 public:
  46         IK_QTask(
  47                 int size,
  48                 bool primary,
  49                 bool active,
  50                 const IK_QSegment *segment
  51         );
  52         virtual ~IK_QTask() {}
  53
  54         int Id() const
  55         { return m_size; }
  56
  57         void SetId(int id)
  58         { m_id = id; }
  59
  60         int Size() const
  61         { return m_size; }
  62
  63         bool Primary() const
  64         { return m_primary; }
  65
  66         bool Active() const
  67         { return m_active; }
  68
  69         MT_Scalar Weight() const
  70         { return m_weight*m_weight; }
  71
  72         void SetWeight(MT_Scalar weight)
  73         { m_weight = sqrt(weight); }
  74
  75         virtual void ComputeJacobian(IK_QJacobian& jacobian)=0;
  76
  77         virtual MT_Scalar Distance() const=0;
  78
  79         virtual bool PositionTask() const { return false; }
  80
  81         virtual void Scale(MT_Scalar) {}
  82
  83 protected:
  84         int m_id;
  85         int m_size;
  86         bool m_primary;
  87         bool m_active;
  88         const IK_QSegment *m_segment;
  89         MT_Scalar m_weight;
  90 };
  91
  92 class IK_QPositionTask : public IK_QTask
  93 {
  94 public:
  95         IK_QPositionTask(
  96                 bool primary,
  97                 const IK_QSegment *segment,
  98                 const MT_Vector3& goal
  99         );
 100
 101         void ComputeJacobian(IK_QJacobian& jacobian);
 102
 103         MT_Scalar Distance() const;
 104
 105         bool PositionTask() const { return true; }
 106         void Scale(MT_Scalar scale) { m_goal *= scale; m_clamp_length *= scale; }
 107
 108 private:
 109         MT_Vector3 m_goal;
 110         MT_Scalar m_clamp_length;
 111 };
 112
 113 class IK_QOrientationTask : public IK_QTask
 114 {
 115 public:
 116         IK_QOrientationTask(
 117                 bool primary,
 118                 const IK_QSegment *segment,
 119                 const MT_Matrix3x3& goal
 120         );
 121
 122         MT_Scalar Distance() const { return m_distance; }
 123         void ComputeJacobian(IK_QJacobian& jacobian);
 124
 125 private:
 126         MT_Matrix3x3 m_goal;
 127         MT_Scalar m_distance;
 128 };
 129
 130
 131 class IK_QCenterOfMassTask : public IK_QTask
 132 {
 133 public:
 134         IK_QCenterOfMassTask(
 135                 bool primary,
 136                 const IK_QSegment *segment,
 137                 const MT_Vector3& center
 138         );
 139
 140         void ComputeJacobian(IK_QJacobian& jacobian);
 141
 142         MT_Scalar Distance() const;
 143
 144         void Scale(MT_Scalar scale) { m_goal_center *= scale; m_distance *= scale; }
 145
 146 private:
 147         MT_Scalar ComputeTotalMass(const IK_QSegment *segment);
 148         MT_Vector3 ComputeCenter(const IK_QSegment *segment);
 149         void JacobianSegment(IK_QJacobian& jacobian, MT_Vector3& center, const IK_QSegment *segment);
 150
 151         MT_Vector3 m_goal_center;
 152         MT_Scalar m_total_mass_inv;
 153         MT_Scalar m_distance;
 154 };
 155
 156 #endif
 157