RNA wrapping of Action Groups:
[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., 59 Temple Place - Suite 330, Boston, MA  02111-1307, 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 #ifndef NAN_INCLUDED_IK_QJacobian_h
32
33 #define NAN_INCLUDED_IK_QJacobian_h
34
35 #include "TNT/cmat.h"
36 #include <vector>
37 #include "MT_Vector3.h"
38
39 class IK_QJacobian
40 {
41 public:
42         typedef TNT::Matrix<MT_Scalar> TMatrix;
43         typedef TNT::Vector<MT_Scalar> TVector;
44
45         IK_QJacobian();
46         ~IK_QJacobian();
47
48         // Call once to initialize
49         void ArmMatrices(int dof, int task_size);
50         void SetDoFWeight(int dof, MT_Scalar weight);
51
52         // Iteratively called
53         void SetBetas(int id, int size, const MT_Vector3& v);
54         void SetDerivatives(int id, int dof_id, const MT_Vector3& v);
55
56         void Invert();
57
58         MT_Scalar AngleUpdate(int dof_id) const;
59         MT_Scalar AngleUpdateNorm() const;
60
61         // DoF locking for inner clamping loop
62         void Lock(int dof_id, MT_Scalar delta);
63
64         // Secondary task
65         bool ComputeNullProjection();
66
67         void Restrict(TVector& d_theta, TMatrix& null);
68         void SubTask(IK_QJacobian& jacobian);
69
70 private:
71         
72         void InvertSDLS();
73         void InvertDLS();
74
75         int m_dof, m_task_size;
76         bool m_transpose;
77
78         // the jacobian matrix and it's null space projector
79         TMatrix m_jacobian, m_jacobian_tmp;
80         TMatrix m_null;
81
82         /// the vector of intermediate betas
83         TVector m_beta;
84
85         /// the vector of computed angle changes
86         TVector m_d_theta;
87
88         /// space required for SVD computation
89
90         TVector m_svd_w;
91         TMatrix m_svd_v;
92         TMatrix m_svd_u;
93     TVector m_work1;
94     TVector m_work2;
95
96         TMatrix m_svd_u_t;
97         TVector m_svd_u_beta;
98
99         // space required for SDLS
100
101         bool m_sdls;
102         TVector m_norm;
103         TVector m_d_theta_tmp;
104         MT_Scalar m_min_damp;
105
106         // null space task vector
107         TVector m_alpha;
108
109         // dof weighting
110         TVector m_weight;
111         TVector m_weight_sqrt;
112 };
113
114 #endif
115