intern/iksolver/intern/IK_Solver.cpp

   1 /**
   2  * $Id$
   3  * ***** BEGIN GPL LICENSE BLOCK *****
   4  *
   5  * This program is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU General Public License
   7  * as published by the Free Software Foundation; either version 2
   8  * of the License, or (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program; if not, write to the Free Software Foundation,
  17  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
  18  *
  19  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
  20  * All rights reserved.
  21  *
  22  * The Original Code is: all of this file.
  23  *
  24  * Original Author: Laurence
  25  * Contributor(s): Brecht
  26  *
  27  * ***** END GPL LICENSE BLOCK *****
  28  */
  29
  30 #include "../extern/IK_solver.h"
  31
  32 #include "IK_QJacobianSolver.h"
  33 #include "IK_QSegment.h"
  34 #include "IK_QTask.h"
  35
  36 #include <list>
  37 using namespace std;
  38
  39 class IK_QSolver {
  40 public:
  41         IK_QSolver() : root(NULL) {};
  42
  43         IK_QJacobianSolver solver;
  44         IK_QSegment *root;
  45         std::list<IK_QTask*> tasks;
  46 };
  47
  48 // FIXME: locks still result in small "residual" changes to the locked axes...
  49 IK_QSegment *CreateSegment(int flag, bool translate)
  50 {
  51         int ndof = 0;
  52         ndof += (flag & IK_XDOF)? 1: 0;
  53         ndof += (flag & IK_YDOF)? 1: 0;
  54         ndof += (flag & IK_ZDOF)? 1: 0;
  55
  56         IK_QSegment *seg;
  57
  58         if (ndof == 0)
  59                 return NULL;
  60         else if (ndof == 1) {
  61                 int axis;
  62
  63                 if (flag & IK_XDOF) axis = 0;
  64                 else if (flag & IK_YDOF) axis = 1;
  65                 else axis = 2;
  66
  67                 if (translate)
  68                         seg = new IK_QTranslateSegment(axis);
  69                 else
  70                         seg = new IK_QRevoluteSegment(axis);
  71         }
  72         else if (ndof == 2) {
  73                 int axis1, axis2;
  74
  75                 if (flag & IK_XDOF) {
  76                         axis1 = 0;
  77                         axis2 = (flag & IK_YDOF)? 1: 2;
  78                 }
  79                 else {
  80                         axis1 = 1;
  81                         axis2 = 2;
  82                 }
  83
  84                 if (translate)
  85                         seg = new IK_QTranslateSegment(axis1, axis2);
  86                 else {
  87                         if (axis1 + axis2 == 2)
  88                                 seg = new IK_QSwingSegment();
  89                         else
  90                                 seg = new IK_QElbowSegment((axis1 == 0)? 0: 2);
  91                 }
  92         }
  93         else {
  94                 if (translate)
  95                         seg = new IK_QTranslateSegment();
  96                 else
  97                         seg = new IK_QSphericalSegment();
  98         }
  99
 100         return seg;
 101 }
 102
 103 IK_Segment *IK_CreateSegment(int flag)
 104 {
 105         IK_QSegment *rot = CreateSegment(flag, false);
 106         IK_QSegment *trans = CreateSegment(flag >> 3, true);
 107
 108         IK_QSegment *seg;
 109
 110         if (rot == NULL && trans == NULL)
 111                 seg = new IK_QNullSegment();
 112         else if (rot == NULL)
 113                 seg = trans;
 114         else {
 115                 seg = rot;
 116
 117                 // make it seem from the interface as if the rotation and translation
 118                 // segment are one
 119                 if (trans) {
 120                         seg->SetComposite(trans);
 121                         trans->SetParent(seg);
 122                 }
 123         }
 124
 125         return seg;
 126 }
 127
 128 void IK_FreeSegment(IK_Segment *seg)
 129 {
 130         IK_QSegment *qseg = (IK_QSegment*)seg;
 131
 132         if (qseg->Composite())
 133                 delete qseg->Composite();
 134         delete qseg;
 135 }
 136
 137 void IK_SetParent(IK_Segment *seg, IK_Segment *parent)
 138 {
 139         IK_QSegment *qseg = (IK_QSegment*)seg;
 140         IK_QSegment *qparent = (IK_QSegment*)parent;
 141
 142         if (qparent && qparent->Composite())
 143                 qseg->SetParent(qparent->Composite());
 144         else
 145                 qseg->SetParent(qparent);
 146 }
 147
 148 void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
 149 {
 150         IK_QSegment *qseg = (IK_QSegment*)seg;
 151
 152         MT_Vector3 mstart(start);
 153         // convert from blender column major to moto row major
 154         MT_Matrix3x3 mbasis(basis[0][0], basis[1][0], basis[2][0],
 155                             basis[0][1], basis[1][1], basis[2][1],
 156                             basis[0][2], basis[1][2], basis[2][2]);
 157         MT_Matrix3x3 mrest(rest[0][0], rest[1][0], rest[2][0],
 158                            rest[0][1], rest[1][1], rest[2][1],
 159                            rest[0][2], rest[1][2], rest[2][2]);
 160         MT_Scalar mlength(length);
 161
 162         if (qseg->Composite()) {
 163                 MT_Vector3 cstart(0, 0, 0);
 164                 MT_Matrix3x3 cbasis;
 165                 cbasis.setIdentity();
 166
 167                 qseg->SetTransform(mstart, mrest, mbasis, 0.0);
 168                 qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
 169         }
 170         else
 171                 qseg->SetTransform(mstart, mrest, mbasis, mlength);
 172 }
 173
 174 void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
 175 {
 176         IK_QSegment *qseg = (IK_QSegment*)seg;
 177
 178         if (axis >= IK_TRANS_X) {
 179                 if(!qseg->Translational()) {
 180                         if(qseg->Composite() && qseg->Composite()->Translational())
 181                                 qseg = qseg->Composite();
 182                         else
 183                                 return;
 184                 }
 185
 186                 if(axis == IK_TRANS_X) axis = IK_X;
 187                 else if(axis == IK_TRANS_Y) axis = IK_Y;
 188                 else axis = IK_Z;
 189         }
 190
 191         qseg->SetLimit(axis, lmin, lmax);
 192 }
 193
 194 void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
 195 {
 196         if (stiffness < 0.0)
 197                 return;
 198
 199         if (stiffness > 0.999)
 200                 stiffness = 0.999;
 201
 202         IK_QSegment *qseg = (IK_QSegment*)seg;
 203         MT_Scalar weight = 1.0-stiffness;
 204
 205         if (axis >= IK_TRANS_X) {
 206                 if(!qseg->Translational()) {
 207                         if(qseg->Composite() && qseg->Composite()->Translational())
 208                                 qseg = qseg->Composite();
 209                         else
 210                                 return;
 211                 }
 212
 213                 if(axis == IK_TRANS_X) axis = IK_X;
 214                 else if(axis == IK_TRANS_Y) axis = IK_Y;
 215                 else axis = IK_Z;
 216         }
 217
 218         qseg->SetWeight(axis, weight);
 219 }
 220
 221 void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
 222 {
 223         IK_QSegment *qseg = (IK_QSegment*)seg;
 224         const MT_Matrix3x3& change = qseg->BasisChange();
 225
 226         if (qseg->Translational() && qseg->Composite())
 227                 qseg = qseg->Composite();
 228
 229         // convert from moto row major to blender column major
 230         basis_change[0][0] = (float)change[0][0];
 231         basis_change[1][0] = (float)change[0][1];
 232         basis_change[2][0] = (float)change[0][2];
 233         basis_change[0][1] = (float)change[1][0];
 234         basis_change[1][1] = (float)change[1][1];
 235         basis_change[2][1] = (float)change[1][2];
 236         basis_change[0][2] = (float)change[2][0];
 237         basis_change[1][2] = (float)change[2][1];
 238         basis_change[2][2] = (float)change[2][2];
 239 }
 240
 241 void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
 242 {
 243         IK_QSegment *qseg = (IK_QSegment*)seg;
 244
 245         if (!qseg->Translational() && qseg->Composite())
 246                 qseg = qseg->Composite();
 247
 248         const MT_Vector3& change = qseg->TranslationChange();
 249
 250         translation_change[0] = (float)change[0];
 251         translation_change[1] = (float)change[1];
 252         translation_change[2] = (float)change[2];
 253 }
 254
 255 IK_Solver *IK_CreateSolver(IK_Segment *root)
 256 {
 257         if (root == NULL)
 258                 return NULL;
 259
 260         IK_QSolver *solver = new IK_QSolver();
 261         solver->root = (IK_QSegment*)root;
 262
 263         return (IK_Solver*)solver;
 264 }
 265
 266 void IK_FreeSolver(IK_Solver *solver)
 267 {
 268         if (solver == NULL)
 269                 return;
 270
 271         IK_QSolver *qsolver = (IK_QSolver*)solver;
 272         std::list<IK_QTask*>& tasks = qsolver->tasks;
 273         std::list<IK_QTask*>::iterator task;
 274
 275         for (task = tasks.begin(); task != tasks.end(); task++)
 276                 delete (*task);
 277
 278         delete qsolver;
 279 }
 280
 281 void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
 282 {
 283         if (solver == NULL || tip == NULL)
 284                 return;
 285
 286         IK_QSolver *qsolver = (IK_QSolver*)solver;
 287         IK_QSegment *qtip = (IK_QSegment*)tip;
 288
 289         if (qtip->Composite())
 290                 qtip = qtip->Composite();
 291
 292         MT_Vector3 pos(goal);
 293
 294         IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
 295         ee->SetWeight(weight);
 296         qsolver->tasks.push_back(ee);
 297 }
 298
 299 void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
 300 {
 301         if (solver == NULL || tip == NULL)
 302                 return;
 303
 304         IK_QSolver *qsolver = (IK_QSolver*)solver;
 305         IK_QSegment *qtip = (IK_QSegment*)tip;
 306
 307         if (qtip->Composite())
 308                 qtip = qtip->Composite();
 309
 310         // convert from blender column major to moto row major
 311         MT_Matrix3x3 rot(goal[0][0], goal[1][0], goal[2][0],
 312                          goal[0][1], goal[1][1], goal[2][1],
 313                          goal[0][2], goal[1][2], goal[2][2]);
 314
 315         IK_QTask *orient = new IK_QOrientationTask(true, qtip, rot);
 316         orient->SetWeight(weight);
 317         qsolver->tasks.push_back(orient);
 318 }
 319
 320 void IK_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float goal[3], float polegoal[3], float poleangle, int getangle)
 321 {
 322         if (solver == NULL || tip == NULL)
 323                 return;
 324
 325         IK_QSolver *qsolver = (IK_QSolver*)solver;
 326         IK_QSegment *qtip = (IK_QSegment*)tip;
 327
 328         MT_Vector3 qgoal(goal);
 329         MT_Vector3 qpolegoal(polegoal);
 330
 331         qsolver->solver.SetPoleVectorConstraint(
 332                 qtip, qgoal, qpolegoal, poleangle, getangle);
 333 }
 334
 335 float IK_SolverGetPoleAngle(IK_Solver *solver)
 336 {
 337         if (solver == NULL)
 338                 return 0.0f;
 339
 340         IK_QSolver *qsolver = (IK_QSolver*)solver;
 341
 342         return qsolver->solver.GetPoleAngle();
 343 }
 344
 345 void IK_SolverAddCenterOfMass(IK_Solver *solver, IK_Segment *root, float goal[3], float weight)
 346 {
 347         if (solver == NULL || root == NULL)
 348                 return;
 349
 350         IK_QSolver *qsolver = (IK_QSolver*)solver;
 351         IK_QSegment *qroot = (IK_QSegment*)root;
 352
 353         // convert from blender column major to moto row major
 354         MT_Vector3 center(goal);
 355
 356         IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center);
 357         com->SetWeight(weight);
 358         qsolver->tasks.push_back(com);
 359 }
 360
 361 int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
 362 {
 363         if (solver == NULL)
 364                 return 0;
 365
 366         IK_QSolver *qsolver = (IK_QSolver*)solver;
 367
 368         IK_QSegment *root = qsolver->root;
 369         IK_QJacobianSolver& jacobian = qsolver->solver;
 370         std::list<IK_QTask*>& tasks = qsolver->tasks;
 371         MT_Scalar tol = tolerance;
 372
 373         if(!jacobian.Setup(root, tasks))
 374                 return 0;
 375
 376         bool result = jacobian.Solve(root, tasks, tol, max_iterations);
 377
 378         return ((result)? 1: 0);
 379 }
 380