doxygen: intern/iksolver tagged. Leaving out TNT, since we have it in multiple locations.
[blender.git] / intern / iksolver / intern / IK_QJacobian.h
1
2 /*
3  * $Id$
4  * ***** BEGIN GPL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software Foundation,
18  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19  *
20  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
21  * All rights reserved.
22  *
23  * The Original Code is: all of this file.
24  *
25  * Original Author: Laurence
26  * Contributor(s): Brecht
27  *
28  * ***** END GPL LICENSE BLOCK *****
29  */
30
31 /** \file iksolver/intern/IK_QJacobian.h
32  *  \ingroup iksolver
33  */
34
35
36 #ifndef NAN_INCLUDED_IK_QJacobian_h
37
38 #define NAN_INCLUDED_IK_QJacobian_h
39
40 #include "TNT/cmat.h"
41 #include <vector>
42 #include "MT_Vector3.h"
43
44 class IK_QJacobian
45 {
46 public:
47         typedef TNT::Matrix<MT_Scalar> TMatrix;
48         typedef TNT::Vector<MT_Scalar> TVector;
49
50         IK_QJacobian();
51         ~IK_QJacobian();
52
53         // Call once to initialize
54         void ArmMatrices(int dof, int task_size);
55         void SetDoFWeight(int dof, MT_Scalar weight);
56
57         // Iteratively called
58         void SetBetas(int id, int size, const MT_Vector3& v);
59         void SetDerivatives(int id, int dof_id, const MT_Vector3& v);
60
61         void Invert();
62
63         MT_Scalar AngleUpdate(int dof_id) const;
64         MT_Scalar AngleUpdateNorm() const;
65
66         // DoF locking for inner clamping loop
67         void Lock(int dof_id, MT_Scalar delta);
68
69         // Secondary task
70         bool ComputeNullProjection();
71
72         void Restrict(TVector& d_theta, TMatrix& null);
73         void SubTask(IK_QJacobian& jacobian);
74
75 private:
76         
77         void InvertSDLS();
78         void InvertDLS();
79
80         int m_dof, m_task_size;
81         bool m_transpose;
82
83         // the jacobian matrix and it's null space projector
84         TMatrix m_jacobian, m_jacobian_tmp;
85         TMatrix m_null;
86
87         /// the vector of intermediate betas
88         TVector m_beta;
89
90         /// the vector of computed angle changes
91         TVector m_d_theta;
92
93         /// space required for SVD computation
94
95         TVector m_svd_w;
96         TMatrix m_svd_v;
97         TMatrix m_svd_u;
98     TVector m_work1;
99     TVector m_work2;
100
101         TMatrix m_svd_u_t;
102         TVector m_svd_u_beta;
103
104         // space required for SDLS
105
106         bool m_sdls;
107         TVector m_norm;
108         TVector m_d_theta_tmp;
109         MT_Scalar m_min_damp;
110
111         // null space task vector
112         TVector m_alpha;
113
114         // dof weighting
115         TVector m_weight;
116         TVector m_weight_sqrt;
117 };
118
119 #endif
120