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 IK_QSegment *CreateSegment(int flag, bool translate)
  49 {
  50         int ndof = 0;
  51         ndof += (flag & IK_XDOF)? 1: 0;
  52         ndof += (flag & IK_YDOF)? 1: 0;
  53         ndof += (flag & IK_ZDOF)? 1: 0;
  54
  55         IK_QSegment *seg;
  56
  57         if (ndof == 0)
  58                 return NULL;
  59         else if (ndof == 1) {
  60                 int axis;
  61
  62                 if (flag & IK_XDOF) axis = 0;
  63                 else if (flag & IK_YDOF) axis = 1;
  64                 else axis = 2;
  65
  66                 if (translate)
  67                         seg = new IK_QTranslateSegment(axis);
  68                 else
  69                         seg = new IK_QRevoluteSegment(axis);
  70         }
  71         else if (ndof == 2) {
  72                 int axis1, axis2;
  73
  74                 if (flag & IK_XDOF) {
  75                         axis1 = 0;
  76                         axis2 = (flag & IK_YDOF)? 1: 2;
  77                 }
  78                 else {
  79                         axis1 = 1;
  80                         axis2 = 2;
  81                 }
  82
  83                 if (translate)
  84                         seg = new IK_QTranslateSegment(axis1, axis2);
  85                 else {
  86                         if (axis1 + axis2 == 2)
  87                                 seg = new IK_QSwingSegment();
  88                         else
  89                                 seg = new IK_QElbowSegment((axis1 == 0)? 0: 2);
  90                 }
  91         }
  92         else {
  93                 if (translate)
  94                         seg = new IK_QTranslateSegment();
  95                 else
  96                         seg = new IK_QSphericalSegment();
  97         }
  98
  99         return seg;
 100 }
 101
 102 IK_Segment *IK_CreateSegment(int flag)
 103 {
 104         IK_QSegment *rot = CreateSegment(flag, false);
 105         IK_QSegment *trans = CreateSegment(flag >> 3, true);
 106
 107         IK_QSegment *seg;
 108
 109         if (rot == NULL && trans == NULL)
 110                 seg = new IK_QNullSegment();
 111         else if (rot == NULL)
 112                 seg = trans;
 113         else {
 114                 seg = rot;
 115
 116                 // make it seem from the interface as if the rotation and translation
 117                 // segment are one
 118                 if (trans) {
 119                         seg->SetComposite(trans);
 120                         trans->SetParent(seg);
 121                 }
 122         }
 123
 124         return seg;
 125 }
 126
 127 void IK_FreeSegment(IK_Segment *seg)
 128 {
 129         IK_QSegment *qseg = (IK_QSegment*)seg;
 130
 131         if (qseg->Composite())
 132                 delete qseg->Composite();
 133         delete qseg;
 134 }
 135
 136 void IK_SetParent(IK_Segment *seg, IK_Segment *parent)
 137 {
 138         IK_QSegment *qseg = (IK_QSegment*)seg;
 139         IK_QSegment *qparent = (IK_QSegment*)parent;
 140
 141         if (qparent && qparent->Composite())
 142                 qseg->SetParent(qparent->Composite());
 143         else
 144                 qseg->SetParent(qparent);
 145 }
 146
 147 void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
 148 {
 149         IK_QSegment *qseg = (IK_QSegment*)seg;
 150
 151         MT_Vector3 mstart(start);
 152         // convert from blender column major to moto row major
 153         MT_Matrix3x3 mbasis(basis[0][0], basis[1][0], basis[2][0],
 154                             basis[0][1], basis[1][1], basis[2][1],
 155                             basis[0][2], basis[1][2], basis[2][2]);
 156         MT_Matrix3x3 mrest(rest[0][0], rest[1][0], rest[2][0],
 157                            rest[0][1], rest[1][1], rest[2][1],
 158                            rest[0][2], rest[1][2], rest[2][2]);
 159         MT_Scalar mlength(length);
 160
 161         if (qseg->Composite()) {
 162                 MT_Vector3 cstart(0, 0, 0);
 163                 MT_Matrix3x3 cbasis;
 164                 cbasis.setIdentity();
 165
 166                 qseg->SetTransform(mstart, mrest, mbasis, 0.0);
 167                 qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
 168         }
 169         else
 170                 qseg->SetTransform(mstart, mrest, mbasis, mlength);
 171 }
 172
 173 void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
 174 {
 175         IK_QSegment *qseg = (IK_QSegment*)seg;
 176
 177         if (axis >= IK_TRANS_X) {
 178                 if(!qseg->Translational()) {
 179                         if(qseg->Composite() && qseg->Composite()->Translational())
 180                                 qseg = qseg->Composite();
 181                         else
 182                                 return;
 183                 }
 184
 185                 if(axis == IK_TRANS_X) axis = IK_X;
 186                 else if(axis == IK_TRANS_Y) axis = IK_Y;
 187                 else axis = IK_Z;
 188         }
 189
 190         qseg->SetLimit(axis, lmin, lmax);
 191 }
 192
 193 void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
 194 {
 195         if (stiffness < 0.0)
 196                 return;
 197
 198         if (stiffness > 0.999)
 199                 stiffness = 0.999;
 200
 201         IK_QSegment *qseg = (IK_QSegment*)seg;
 202         MT_Scalar weight = 1.0-stiffness;
 203
 204         if (axis >= IK_TRANS_X) {
 205                 if(!qseg->Translational()) {
 206                         if(qseg->Composite() && qseg->Composite()->Translational())
 207                                 qseg = qseg->Composite();
 208                         else
 209                                 return;
 210                 }
 211
 212                 if(axis == IK_TRANS_X) axis = IK_X;
 213                 else if(axis == IK_TRANS_Y) axis = IK_Y;
 214                 else axis = IK_Z;
 215         }
 216
 217         qseg->SetWeight(axis, weight);
 218 }
 219
 220 void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
 221 {
 222         IK_QSegment *qseg = (IK_QSegment*)seg;
 223         const MT_Matrix3x3& change = qseg->BasisChange();
 224
 225         if (qseg->Translational() && qseg->Composite())
 226                 qseg = qseg->Composite();
 227
 228         // convert from moto row major to blender column major
 229         basis_change[0][0] = (float)change[0][0];
 230         basis_change[1][0] = (float)change[0][1];
 231         basis_change[2][0] = (float)change[0][2];
 232         basis_change[0][1] = (float)change[1][0];
 233         basis_change[1][1] = (float)change[1][1];
 234         basis_change[2][1] = (float)change[1][2];
 235         basis_change[0][2] = (float)change[2][0];
 236         basis_change[1][2] = (float)change[2][1];
 237         basis_change[2][2] = (float)change[2][2];
 238 }
 239
 240 void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
 241 {
 242         IK_QSegment *qseg = (IK_QSegment*)seg;
 243
 244         if (!qseg->Translational() && qseg->Composite())
 245                 qseg = qseg->Composite();
 246
 247         const MT_Vector3& change = qseg->TranslationChange();
 248
 249         translation_change[0] = (float)change[0];
 250         translation_change[1] = (float)change[1];
 251         translation_change[2] = (float)change[2];
 252 }
 253
 254 IK_Solver *IK_CreateSolver(IK_Segment *root)
 255 {
 256         if (root == NULL)
 257                 return NULL;
 258
 259         IK_QSolver *solver = new IK_QSolver();
 260         solver->root = (IK_QSegment*)root;
 261
 262         return (IK_Solver*)solver;
 263 }
 264
 265 void IK_FreeSolver(IK_Solver *solver)
 266 {
 267         if (solver == NULL)
 268                 return;
 269
 270         IK_QSolver *qsolver = (IK_QSolver*)solver;
 271         std::list<IK_QTask*>& tasks = qsolver->tasks;
 272         std::list<IK_QTask*>::iterator task;
 273
 274         for (task = tasks.begin(); task != tasks.end(); task++)
 275                 delete (*task);
 276
 277         delete qsolver;
 278 }
 279
 280 void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
 281 {
 282         if (solver == NULL || tip == NULL)
 283                 return;
 284
 285         IK_QSolver *qsolver = (IK_QSolver*)solver;
 286         IK_QSegment *qtip = (IK_QSegment*)tip;
 287
 288         if (qtip->Composite())
 289                 qtip = qtip->Composite();
 290
 291         MT_Vector3 pos(goal);
 292
 293         IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
 294         ee->SetWeight(weight);
 295         qsolver->tasks.push_back(ee);
 296 }
 297
 298 void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
 299 {
 300         if (solver == NULL || tip == NULL)
 301                 return;
 302
 303         IK_QSolver *qsolver = (IK_QSolver*)solver;
 304         IK_QSegment *qtip = (IK_QSegment*)tip;
 305
 306         if (qtip->Composite())
 307                 qtip = qtip->Composite();
 308
 309         // convert from blender column major to moto row major
 310         MT_Matrix3x3 rot(goal[0][0], goal[1][0], goal[2][0],
 311                          goal[0][1], goal[1][1], goal[2][1],
 312                          goal[0][2], goal[1][2], goal[2][2]);
 313
 314         IK_QTask *orient = new IK_QOrientationTask(true, qtip, rot);
 315         orient->SetWeight(weight);
 316         qsolver->tasks.push_back(orient);
 317 }
 318
 319 void IK_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float goal[3], float polegoal[3], float poleangle, int getangle)
 320 {
 321         if (solver == NULL || tip == NULL)
 322                 return;
 323
 324         IK_QSolver *qsolver = (IK_QSolver*)solver;
 325         IK_QSegment *qtip = (IK_QSegment*)tip;
 326
 327         MT_Vector3 qgoal(goal);
 328         MT_Vector3 qpolegoal(polegoal);
 329
 330         qsolver->solver.SetPoleVectorConstraint(
 331                 qtip, qgoal, qpolegoal, poleangle, getangle);
 332 }
 333
 334 float IK_SolverGetPoleAngle(IK_Solver *solver)
 335 {
 336         if (solver == NULL)
 337                 return 0.0f;
 338
 339         IK_QSolver *qsolver = (IK_QSolver*)solver;
 340
 341         return qsolver->solver.GetPoleAngle();
 342 }
 343
 344 void IK_SolverAddCenterOfMass(IK_Solver *solver, IK_Segment *root, float goal[3], float weight)
 345 {
 346         if (solver == NULL || root == NULL)
 347                 return;
 348
 349         IK_QSolver *qsolver = (IK_QSolver*)solver;
 350         IK_QSegment *qroot = (IK_QSegment*)root;
 351
 352         // convert from blender column major to moto row major
 353         MT_Vector3 center(goal);
 354
 355         IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center);
 356         com->SetWeight(weight);
 357         qsolver->tasks.push_back(com);
 358 }
 359
 360 int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
 361 {
 362         if (solver == NULL)
 363                 return 0;
 364
 365         IK_QSolver *qsolver = (IK_QSolver*)solver;
 366
 367         IK_QSegment *root = qsolver->root;
 368         IK_QJacobianSolver& jacobian = qsolver->solver;
 369         std::list<IK_QTask*>& tasks = qsolver->tasks;
 370         MT_Scalar tol = tolerance;
 371
 372         if(!jacobian.Setup(root, tasks))
 373                 return 0;
 374
 375         bool result = jacobian.Solve(root, tasks, tol, max_iterations);
 376
 377         return ((result)? 1: 0);
 378 }
 379